# Risk-Averse Receding Horizon Motion Planning

Anushri Dixit<EMAIL_ADDRESS>Mohamadreza Ahmadi<EMAIL_ADDRESS>Joel W. Burdick<EMAIL_ADDRESS>1200 E California Blvd, Pasadena, CA
91125

###### Abstract

This paper studies the problem of risk-averse receding horizon motion planning
for agents with uncertain dynamics, in the presence of stochastic, dynamic
obstacles. We propose a model predictive control (MPC) scheme that formulates
the obstacle avoidance constraint using coherent risk measures. To handle
disturbances, or process noise, in the state dynamics, the state constraints
are tightened in a risk-aware manner to provide a disturbance feedback policy.
We also propose a waypoint following algorithm that uses the proposed MPC
scheme for discrete distributions and prove its risk-sensitive recursive
feasibility while guaranteeing finite-time task completion. We further
investigate some commonly used coherent risk metrics, namely, conditional
value-at-risk (CVaR), entropic value-at-risk (EVaR), and g-entropic risk
measures, and propose a tractable incorporation within MPC. We illustrate our
framework via simulation studies.

###### keywords:

Coherent risk measures, model predictive control, stochastic control, motion
planning, obstacle avoidance, distributional robustness.

††journal: Aritificial Intelligence Journal

##  1 Introduction

Autonomous robots must increasingly plan motions in unstructured and uncertain
environments with safety guarantees. Some applications where safe planning is
required include autonomous traversal over extreme terrain in GPS-denied
subterranean environments [1, 2], inspection of planetary environments [3],
search and rescue missions caused by natural disasters [4, 5], and autonomous
driving [6]. These applications present challenges at all the levels of
planning and control [7]. The lowest control level requires a good physical
model for accurate motion prediction. To ensure robustness and safety, these
models are often equipped with low-level controllers that leverage tools from
robust control and invariant set theory [8]. At the intermediate level,
algorithms must plan paths that are dynamically feasible, obstacle-free, and
account for uncertainty in the motion dynamics, sensor measurements, and the
environment. Several existing algorithms (model predictive control and
A*-based graph planners to name a few) tackle some or all of these issues [9,
10]. At the highest level, robots must reason about their (uncertain)
environment and decide on what tasks to do. Partially Observable Markov
Decision Processes (POMDPs) are popular models for such sequential planning
tasks [11]. Our work looks at the problem of obstacle avoidance using model
predictive control (MPC) techniques.

MPC is widely used for robotic motion planning because it incorporates robot
dynamics and state and control constraints in a receding horizon fashion [12,
13]. There are many ways to incorporate uncertainty in MPC. Robust MPC
accounts for worst-case disturbances in a set of bounded uncertainties [14].
This approach is often too conservative, since it does not account for the
distribution of the uncertainties. Stochastic MPC [15] minimizes the expected
value of a cost function, while respecting a bound on the probability of
violating the state and control constraints. The chance constraints in
stochastic MPC do not usefully account for events in the tail of the
uncertainty distribution, and the policy that results from an expected cost
function minimizes the cost on average. In this work, we optimize for policies
that have risk-averse behavior: the policies are not as conservative as in the
robust case but account for “risky” outcomes in the tail of the uncertainty
distribution and therefore perform better in practice.

There are many ways to incorporate risk into a control strategy [16], such as
chance constraints [17, 18], exponential utility functions [19], and
distributional robustness [20, 21]. However, applications in autonomy and
robotics require more “nuanced assessments of risk” [22]. Artzner et. al. [23]
characterized a set of coherent risk measures that have natural and desirable
properties. This paper focuses on measures which are widely used in finance
and operations research, among other fields.

Motion planning based on coherent risk measures have previously been
considered. In [24], the authors provided an MPC scheme for a discrete-time
dynamical system with process noise whose objective was a Conditional Value-
at-Risk (CVaR) measure. They further provided Lyapunov conditions for risk-
sensitive exponential stability. In [25], the authors devised an MPC scheme to
avoid randomly moving obstacles using a CVaR risk metric. Similar results were
obtained in [26] on Entropic Value-at-Risk (EVaR) metric for obstacle
avoidance with additional guarantees of recursive feasibility and finite-time
task completion while following a set of waypoints. Risk-sensitive obstacle
avoidance has also been tackled through CVaR-based control barrier functions
in [27] with application to bipedal robot locomotion. In [28], the authors
considered multistage risk-averse and risk-constrained optimal control for
general coherent risk measures with conic representations. A scenario tree-
based branch MPC framework with feedback policies that account for a tradeoff
between robustness and performance through CVaR metrics was proposed in [29].

This paper provides a framework for risk-averse model predictive control with
obstacle avoidance constraints. This work is an extension of previous work
[26] that allowed for randomly moving obstacles while using entropic value-at-
risk as the risk metric. This paper allows for a linear discrete-time system
to be affected by both process noise as well as measurement noise in the
sensing of obstacle position and orientation. The control input is
parameterized as a disturbance feedback policy. Additionally, the MPC scheme
in this work allows for a general class of coherent risk measures. Coherent
risk measures can be expressed as a distributionally-robust expectation, i.e,
the risk is equivalently expressed as the worst-case expectation over a
convex, closed set of distributions. We use this property of distributional
robustness extensively throughout this paper. We reformulate the risk-aware
MPC with obstacle avoidance constraints as a convex, mixed-integer program.

This paper is organized as follows. First, we review relevant facts on
coherent risk measures and some commonly used examples of these measures in
Section 2. Section 3 presents the main problem studied in this paper. Section
4 proposes a problem reformulation based on convex mixed integer programming
to solve the risk-averse receding horizon path planning problem. It also
discusses the properties of feasibility and finite-time task completion with
some confidence. Section 5 illustrates the method via numerical experiments.
We conclude with a discussion of our contributions and avenues of future work
in Section 6. The Appendix contains the proofs not presented in the preceding
sections.

Notation:  We denote by $\mathbb{R}^{n}$ the $n$-dimensional Euclidean space,
$\mathbb{R}_{\geq 0}$ the non-negative reals, and $\mathbb{Z}_{\geq 0}$ the
set of non-negative integers. The index set, $\\{k,k+1,\dotsc,k+N\\}$ is
denoted by $\mathbb{Z}_{k}^{k+N}$. Throughout the paper, a bold font denotes a
vector and $(\cdot)^{\top}$ is its transpose, e.g.,
$\boldsymbol{a}=(a_{1},\ldots,a_{n})^{\top}$, with $n\in\\{1,2,\ldots\\}$. For
vector $\boldsymbol{a}$, we use $\boldsymbol{a}\succeq(\preceq)\boldsymbol{0}$
to denote element-wise non-negativity (non-positivity),
$\boldsymbol{a}\equiv\boldsymbol{0}$ to show all elements of $\boldsymbol{a}$
are zero, and $|\boldsymbol{a}|$ to denote the element-wise absolute value of
$\boldsymbol{a}$. For vectors $a,b\in\mathbb{R}^{n}$, we denote their inner
product by $\langle\boldsymbol{a},\boldsymbol{b}\rangle$, i.e.,
$\langle\boldsymbol{a},\boldsymbol{b}\rangle=\boldsymbol{a}^{\top}\boldsymbol{b}$.
For a finite set $\mathcal{A}$, denote its power set $2^{\mathcal{A}}$. For a
probability space $(\Omega,\mathcal{F},\mathbb{P})$ and a constant
$p\in[1,\infty)$, $\mathcal{L}_{p}(\Omega,\mathcal{F},\mathbb{P})$ denotes the
vector space of real valued random variables $X$ for which
$\mathbb{E}|X|^{p}<\infty$. For probability density functions $P(X)$ and
$Q(X)$, $P\ll Q$ implies that $P$ is absolutely continuous with respect to
$Q$, i.e., if $Q(X)=0$, then $P(X)=0$.

We follow the following convention for indices that appear most commonly as
sub/superscripts in this paper:

  * 1.

$k$ indicates the state at time $t+k$,
$\boldsymbol{x}_{k}=\boldsymbol{x}(t+k|t)$,

  * 2.

$j$ denotes the values associated with the $j^{\text{th}}$ possible occurrence
of the random variable (from the sample space),

  * 3.

$l,i$ represent the $l^{\text{th}}$ obstacle and the $i^{\text{th}}$ edge of
the obstacle respectively.

##  2 Preliminaries

We formally consider a probability space $(\Omega,\mathcal{F},{P})$, where
$\Omega$, $\mathcal{F}$, and ${P}$ are the sample space, $\sigma$-algebra over
$\Omega$, and probability measure over $\mathcal{F}$ respectively. In this
paper, a random variable $X:\Omega\xrightarrow[]{}\mathbb{R}$ denotes the cost
of each outcome. The set of all cost random variables defined on $\Omega$ is
given by $\mathfrak{F}$. A risk measure is a function that maps a cost random
variable to a real number, $\rho:\mathfrak{F}\xrightarrow[]{}\mathbb{R}$.

For constrained stochastic optimization programs, chance constraints can be
reformulated by a commonly used risk measure called the Value-at-Risk (VaR).
For a given confidence level $\alpha\in(0,1)$, $\mathrm{VaR}_{1-\alpha}$
denotes the $({1-\alpha})$-quantile value of the cost variable $X$ and is
defined as,

$\displaystyle\text{VaR}_{1-\alpha}(X):=\inf\\{z\,|\,\mathbb{P}(X\leq
z)\geq\alpha\\}.$

It follows that $\text{VaR}_{1-\alpha}(X)\leq 0\implies\mathbb{P}(X\leq
0)\geq\alpha.$ However, VaR is generally nonconvex and hard to compute. We now
introduce convex and monotonic risk measures. In particular, we are interested
in coherent risk measures [23] that satisfy the following properties.

######  Definition 1 (Coherent Risk Measures).

Consider two random variables, $X$, $X^{\prime}\in\mathfrak{F}$. A coherent
risk measure, $\rho:\mathfrak{F}\xrightarrow[]{}\mathbb{R}$, is a risk measure
that satisfies the following properties:

  1. 1.

Monotonicity $X\leq X^{\prime}\implies\rho(X)\leq\rho(X^{\prime})$,

  2. 2.

Translational invariance $\rho(X+c)=\rho(X)+c,\,\forall c\in\mathbb{R}$,

  3. 3.

Positive homogeneity $\rho(\alpha X)=\alpha\rho(X),\,\forall\alpha\geq 0$,

  4. 4.

Subadditivity $\rho(X+X^{\prime})\leq\rho(X)+\rho(X^{\prime})$.

Another nice property of coherent risk measures is that they can be written as
the worst-case expectation over a convex, bounded, and closed set of
probability mass (or density) functions (pdf/pmf). This is the dual
representation of a risk measure, and this set is referred to as the risk
envelope.

######  Definition 2 (Representation Theorem).

Every coherent risk measure can be represented in its dual form as,

$\displaystyle\rho(X):=\sup_{Q\in\mathcal{Q}}\mathbb{E}_{Q}(X),$

where the risk envelope $\mathcal{Q}\subset\\{Q\ll{P}\\}$ is convex and
closed.

While coherent risk measures act on a one-dimensional cost random variable, in
this paper, we write $\rho(\boldsymbol{X})$, where $\boldsymbol{X}$ is a
vector of cost random variables of length $n$, to mean
$\rho(\boldsymbol{X})=\begin{bmatrix}\rho(X_{1}),\dotsc,\rho(X_{n})\end{bmatrix}^{T}$.

We next review some examples of coherent risk measures and their dual
representation. We will apply our results to these examples.

###  2.1 Conditional Value-at-Risk

For a given confidence level $\alpha\in(0,1)$, value-at-risk
$\mathrm{VaR}_{1-\alpha}$ denotes the $({1-\alpha})$-quantile value of the
cost variable $X\in\mathcal{L}_{p}(\Omega,\mathcal{F},\mathbb{P})$. The
conditional value-at-risk $\mathrm{CVaR}_{1-\alpha}$ measures the expected
loss in the $({1-\alpha})$-tail given that the threshold
$\mathrm{VaR}_{1-\alpha}$ has been crossed. $\mathrm{CVaR}_{1-\alpha}$ is
found as

$\displaystyle\mathrm{CVaR}_{1-\alpha}(X):=$
$\displaystyle\inf_{z\in\mathbb{R}}\mathbb{E}\Bigg{[}z+\frac{(X-z)^{+}}{1-\alpha}\Bigg{]},$
(1)

where $(\cdot)^{+}=\max\\{\cdot,0\\}$. A value of $\alpha\simeq 0$ corresponds
to a risk-neutral case; whereas, a value of $\alpha\to 1$ is rather a risk-
averse case. $\mathcal{Q}$ is the risk envelope defined by,

$\mathcal{Q}:=\Big{\\{}Q\ll
P\,|\,0\leq\frac{dQ}{dP}\leq\frac{1}{\alpha}\Big{\\}},$ (2)

where $\frac{dQ}{dP}$ is called the Radon–Nikodym derivative and it gives the
rate of change of density of one density function, $Q$, w.r.t the other, $P$.
For a discrete probability distribution, the risk envelope translates to

$\mathcal{Q}:=\Big{\\{}q\in\Delta_{J}\,|\,0\leq
q(j)\leq\frac{p(j)}{\alpha}\,\forall j\in\\{1,\dots,J\\}\Big{\\}}$ (3)

where $\Delta_{J}$ is the probability simplex,
$\Delta_{J}:=\\{q\in\mathbb{R}^{J}\,|\,q\geq 0,\,\sum_{j=1}^{J}q(j)=1\\}$.

###  2.2 Entropic Value-at-Risk

EVaR, derived using the Chernoff inequality for VaR, is the tightest upper
bound for VaR and CVaR. The $\mathrm{EVaR}_{1-\alpha}$ of random variable $X$
is given by

$\mathrm{EVaR}_{1-\alpha}(X):=\inf_{z>0}\Bigg{[}z^{-1}\ln\frac{\mathbb{E}[e^{Xz}]}{1-\alpha}\Bigg{]}.$
(4)

Similar to $\mathrm{CVaR}_{1-\alpha}$, for $\mathrm{EVaR}_{1-\alpha}$, the
limit $\alpha\to 0$ corresponds to a risk-neutral case; whereas, $\alpha\to 1$
corresponds to a risk-averse case. In fact, it was demonstrated in [30,
Proposition 3.2] that $\lim_{{\alpha}\to
1}\mathrm{EVaR}_{{1-\alpha}}(X)=\operatorname*{ess\,sup}(X)$, where
$\operatorname*{ess\,sup}(X)$ is the worst case value of $X$.

For EVaR, the risk envelope $\mathcal{Q}$ for a continuous random variable
with the pdf $P$ is defined as the epigraph of the KL divergence,

${\mathcal{Q}:=\Big{\\{}Q\ll
P\,|\,D_{KL}(Q||P):=\int\frac{dQ}{dP}\big{(}\ln\frac{dQ}{dP}\big{)}dP\leq-\ln(1-\alpha)\Big{\\}},}$
(5)

where $D_{KL}(Q||P)$ denotes the KL divergence between the distributions $Q$
and $P$. For some $x,y\in\mathbb{R}$, $D_{KL}(x||y)$ can be written in the
form of the exponential cone, $K_{exp}$:

$t\geq x\ln(x/y)\iff(y,x,-t)\in K_{exp}.$

Similarly, for a discrete random variable $X\in\\{x_{1},x_{2},\dotsc,x_{J}\\}$
with pmf given by $p=[p(1),p(2),\dotsc,p(J)]^{T}$, where
$p(j)=\mathbb{P}(X=x_{j}),j\in\mathbb{Z}_{1}^{J}$, the KL divergence is given
as

$D_{KL}(q||p):=\sum_{j=1}^{J}q(j)\ln\bigg{(}\frac{q(j)}{p(j)}\bigg{)},\,\,q,p\in\Delta_{J}=\\{q\in\mathbb{R}^{J}\,|\,q\geq
0,\,\sum_{j=1}^{J}q(j)=1\\}.$

$\mathrm{VaR}_{1-\alpha}(\zeta)$$\mathbb{E}(\zeta)$$\mathrm{CVaR}_{1-\alpha}(\zeta)$$\mathrm{EVaR}_{1-\alpha}(\zeta)$Probability
$1-\alpha$$\zeta$$p(\zeta)$

Figure 1: Comparison of the mean, VaR, and CVaR for a given confidence
$\alpha\in(0,1)$. The axes denote the values of the stochastic variable
$\zeta$, i.e., the minimum distance to the safe set as defined in (11), and
with pdf $p(\zeta)$. The shaded area denotes the $\%(1-\alpha)$ of the area
under $p(\zeta)$. If the goal is to minimize $\zeta$, using
$\mathbb{E}(\zeta)$ as a performance measure is misleading because tail events
with low probability of occurrence are ignored. VaR gives the value of $\zeta$
at the $(1-\alpha)$-tail of the distribution. But, it ignores the values of
$\zeta$ with probability below $1-\alpha$. CVaR is the average of the values
of VaR with probability less than $1-\alpha$ (average of the worst-case values
of $\zeta$ in the $1-\alpha$ tail of the distribution). Note that
$\mathbb{E}(\zeta)\leq\mathrm{VaR}_{1-\alpha}(\zeta)\leq\mathrm{CVaR}_{1-\alpha}(\zeta)\leq\mathrm{EVaR}_{1-\alpha}(\zeta)$.
Hence, $\mathrm{EVaR}_{1-\alpha}(\zeta)$ is a more risk-sensitive measure.

###  2.3 g-entropic risk measures

Let $g$ be a convex function with $g(1)=0$, and $\beta$ be a nonnegative
number. The g-entropic risk measure [30], $\text{ER}_{g,\beta}$, with
divergence level $\beta$ for a random variable
$X\in\mathcal{L}_{p}(\Omega,\mathcal{F},\mathbb{P})$ is defined as,

$\displaystyle\text{ER}_{g,\beta}(X):=\sup_{Q\in\mathcal{Q}}E_{Q}(X),$ (6)

where, $\mathcal{Q}=\\{Q\ll P:\int
g\big{(}\frac{dQ}{dP}\big{)}\,dP\leq\beta\\}$.

The definition (6) describes the g-entropic risk measures in terms of their
dual representation. To obtain the primal form, we can use the generalized
Donsker-Vardhan variational formula [30],

$\displaystyle\inf_{\mu\in\mathbb{R}}\\{\mu+E_{P}(g^{*}(X-\mu))\\}=\sup_{Q\ll
P}\\{E_{Q}(X)-g\big{(}\frac{dQ}{dP}\big{)}\,dP\\},$

where $g^{*}$ is the conjugate (the Legendre-Fenchel transform) of $g$. Both
CVaR and EVaR have been proven to be g-entropic risk measures. Another
g-entropic risk measure that we’ll consider in this work is using the total
variation distance [31]:

$\text{TVD}_{\alpha}(X)=\sup_{Q\in\mathcal{Q}}E_{Q}(X)=\alpha\sup_{x\in\Omega}x+(1-\alpha)\text{CVaR}_{1-\alpha}(X)$

where the risk envelope is given by,

$\mathcal{Q}:=\Big{\\{}q\in\Delta_{J}:\frac{1}{2}\sum_{j=1}^{J}|q(j)-p(j)|\leq\alpha\Big{\\}}.$

##  3 Problem Statement

We consider a class of discrete-time dynamical systems given by

$\displaystyle\boldsymbol{x}(t+1)$
$\displaystyle=A\boldsymbol{x}(t)+B\boldsymbol{u}(t)+D\boldsymbol{\delta}(t),$
(7) $\displaystyle\boldsymbol{y}(t)$ $\displaystyle=C\boldsymbol{x}(t),$

where $\boldsymbol{x}(t)\in\mathbb{R}^{n_{x}}$,
$\boldsymbol{y}(t)\in\mathbb{R}^{n_{y}}$, and
$\boldsymbol{u}(t)\in\mathbb{R}^{n_{u}}$ are the system state, output, and
controls at time $t$, respectively. The system is affected by a stochastic,
additive, process noise $\boldsymbol{\delta}(t)\in\mathbb{R}^{n_{x}}$. In
fact, the noise term $\boldsymbol{\delta}$ can represent exogenous
disturbances or unmodeled dynamics (see the case study in [27] for such
modeling method applied to bipedal robots). We posit the following assumption
about the process noise.

######  Assumption 1 (Discrete process noise).

The process noise $\boldsymbol{\delta}$ is assumed to consist of i.i.d.
samples of a discrete distribution given by the probability mass function
(pmf),
$p_{\delta}=[p_{\delta}(1),p_{\delta}(2),\dotsc,p_{\delta}(J_{\delta})]^{T}$.
For this distribution, we also define the index set
$\mathcal{D}=\mathbb{Z}_{1}^{J_{\delta}}$.

We also consider $L$ moving obstacles with index $l\in\mathbb{Z}_{1}^{L}$ that
can be approximated by a convex polytope defined by $m_{l}$ half-spaces in
$\mathbb{R}^{n_{x}}$

$\bar{\mathcal{O}_{l}}(t)=\\{\boldsymbol{o}\in\mathbb{R}^{n_{x}}\,|\,\boldsymbol{c}_{i,l}^{T}(\boldsymbol{o}-\boldsymbol{a}_{l}(t))\leq\boldsymbol{d}_{i,l},\,\forall
i\in\mathbb{Z}_{1}^{m_{l}}\\}.$ (8)

We allow each polytopic obstacle $\bar{\mathcal{O}_{l}}$,
$l\in\mathbb{Z}_{1}^{L}$, centered at $\boldsymbol{a}_{l}$ at time $t$ to move
randomly w.r.t. the nominal trajectory. The random set defining obstacle
$\bar{\mathcal{O}_{l}}$, $l\in\mathbb{Z}_{1}^{L}$, at time $t$ can be written
as a random rotation $R_{l}$ and random translation $w_{l}$ of
$\bar{\mathcal{O}_{l}}$. Hence, we can rewrite the obstacle at time $t$ as a
random set, $\mathcal{O}_{l}$, as

$\displaystyle\mathcal{O}_{l}(t)$
$\displaystyle=R_{l}(t)\bar{\mathcal{O}_{l}}(t)+\boldsymbol{w}_{l}(t)$ (9)
$\displaystyle=\bigg{\\{}\boldsymbol{o^{\prime}}=R_{l}(t)(\boldsymbol{o}-\boldsymbol{a}_{l}(t))+\boldsymbol{a}_{l}(t)+\boldsymbol{w}_{l}(t)\,|\,\boldsymbol{c}_{i,l}^{T}\boldsymbol{o}\leq\boldsymbol{d}_{i,l},\,\forall
i\in\mathbb{Z}_{1}^{m_{l}}\bigg{\\}}$
$\displaystyle=\bigg{\\{}\boldsymbol{o^{\prime}}\,|\,\boldsymbol{c}_{i,l}^{T}\Big{(}R_{l}(t)^{-1}\big{(}\boldsymbol{o^{\prime}}-\boldsymbol{a}_{l}(t)-\boldsymbol{w}_{l}(t)\big{)}+\boldsymbol{a}_{l}(t)\Big{)}\leq\boldsymbol{d}_{i,l},\,\forall
i\in\mathbb{Z}_{1}^{m_{l}}\bigg{\\}}.$

In other words, we allow the $l^{\text{th}}$ obstacle moving with the nominal
trajectory $\boldsymbol{a}_{l}(t)$ to rotate and translate randomly. The
random movement of the obstacle about the nominal trajectory is described by
the set $\mathcal{O}_{l}(t)$ in the above equations.

Figure 2: A representation of the environment for the motion planning problem.
The light blue polytopes, $\bar{\mathcal{O}}(t)$, represent the nominal
obstacle set centered at the nominal trajectory $\boldsymbol{a}_{l}(t)$. We
allow for random rotations and translations about this trajectory. This random
obstacle set is given by the darker blue polytope $\mathcal{O}(t)$. The safe
set, $\mathcal{S}_{l}(t)$, is the region outside the obstacle set. The goal of
the drone in the figure is to plan a path to the terminal set
$\mathcal{X}_{F}$.

######  Assumption 2 (Discrete measurement noise).

The random rotations and translations are sampled from a joint probability
distribution whose sample space has cardinality $J_{o}$, i.e.,
$\Omega_{l}=\\{(R_{l}^{1},\boldsymbol{w}_{l}^{1}),\dotsc,(R_{l}^{J},\boldsymbol{w}_{l}^{J_{o}})\\}$.
A random rotation and translation is picked from this set with pmf given by
$p_{l}=[p_{l}(1),p_{l}(2),\dotsc,p_{l}(J_{o})]^{T}$. For this distribution, we
also define the index set $\mathcal{J}=\\{1,\dotsc,J_{o}\\}$.

The nominal safe set is defined as the region outside of the polytopic
obstacles

$\displaystyle\bar{\mathcal{S}_{l}}(t)$
$\displaystyle=\mathbb{R}^{n_{y}}\backslash\bar{\mathcal{O}_{l}}(t)$ (10)
$\displaystyle=\big{\\{}\boldsymbol{o}\,|\,\exists
i\in\mathbb{Z}_{1}^{m_{l}},~{}~{}c_{i,l}^{T}(\boldsymbol{o}-\boldsymbol{a}_{l}(t))\geq
d_{i,l}\big{\\}}.$

Similarly, the random safe set is given by
$\mathcal{S}_{l}(t)=\mathbb{R}^{n_{y}}\backslash\mathcal{O}_{l}(t)$. For
obstacle avoidance, we are interested in the minimum distance to the safe set,

$\zeta(\boldsymbol{y}(t),\mathcal{S}_{l}(t))=\text{dist}(\boldsymbol{y}(t),\mathcal{S}_{l}(t)):=\min_{\boldsymbol{z}\in\mathcal{S}_{l}(t)}||\boldsymbol{y}(t)-\boldsymbol{z}||.$
(11)

Our goal is to bound the risk of collision with the randomly moving obstacles
by evaluating the distance from the probabilistic safe set (which is the
complement of the obstacle set) and constraining it to be below a threshold,
$\epsilon_{l}$,

$\mathrm{\rho}_{1-\alpha}\big{[}\zeta(\boldsymbol{y}(t),\mathcal{S}_{l}(t))]\leq\epsilon_{l},\quad\forall
l\in\mathbb{Z}_{1}^{L}.$ (12)

The obstacle avoidance constraint (12) is a risk safety constraint with
confidence level $\alpha$ and risk tolerance (also referred to as risk
threshold) $\epsilon_{l}$ for each obstacle $l\in\mathbb{Z}_{1}^{L}$. Note
that this implies that we allow the coherent risk of the distance from the
safe set to be at most $\epsilon_{l}$ in $1-\alpha$ worst realizations.
Henceforth, we represent $\rho_{1-\alpha}$ as $\rho$ for simplicity.

Let the state constraints take the form
$\mathcal{X}:=\\{x\in\mathbb{R}^{n_{x}}|F_{x}x\leq
g_{x}\\},F_{x}\in\mathbb{R}^{r\times n_{x}},g_{x}\in\mathbb{R}^{r}$, which can
represent physical constraints on a robot. Given that the system is subject to
noise $\delta$, we want to satisfy the following state constraints in the
risk-averse sense:

$\displaystyle\rho(f_{x,n}^{T}x(t+k|t)-g_{x,n})$
$\displaystyle\leq\epsilon_{x},\quad\forall
k\in\mathbb{Z}_{1}^{N},n\in\mathbb{Z}_{1}^{r},$ (13)

where
$F_{x}=\begin{bmatrix}f_{x,1}^{T}&f_{x,2}^{T}&\dotsc&f_{x,r}^{T}\end{bmatrix}^{T},g_{x}=\begin{bmatrix}g_{x,1}^{T}&g_{x,2}^{T}&\dotsc&g_{x,r}^{T}\end{bmatrix}^{T}$.
We write this constraint in shorthand as,
$\rho(\boldsymbol{x}_{k}\not\in\mathcal{X})\leq\epsilon_{x}$.

Similarly, we consider control constraints of the form
$\mathcal{U}:=\\{\boldsymbol{u}\in\mathbb{R}^{n_{u}}|F_{u}\boldsymbol{u}\leq
g_{u}\\},F_{u}\in\mathbb{R}^{s\times n_{u}},g_{u}\in\mathbb{R}^{s}$, for
example, representing actuator limitations, and we want to satisfy the
following risk constraint

$\displaystyle\rho(f_{u,n}^{T}u(t+k|t)-g_{u,n})\leq\epsilon_{u},\quad\forall
k\in\mathbb{Z}_{0}^{N-1},n\in\mathbb{Z}_{1}^{s},$ (14)

where
$F_{u}=\begin{bmatrix}f_{u,1}^{T}&f_{u,2}^{T}&\dotsc&f_{u,s}^{T}\end{bmatrix}^{T},g_{u}=\begin{bmatrix}g_{u,1}^{T}&g_{u,2}^{T}&\dotsc&g_{u,s}^{T}\end{bmatrix}^{T}$.
We write this constraint in shorthand as,
$\rho(\boldsymbol{u}_{k}\not\in\mathcal{U})\leq\epsilon_{u}$.

Note that the uncertainty in the control input $\boldsymbol{u}$ arises from
the description of the control policy as a function of the disturbances. This
disturbance feedback policy will be elaborated upon shortly. If we choose to
have hard constraints on the control input, the risk level can be set to a
conservative value, $\alpha\rightarrow 1$, for the control constraints. For
ease of presentation, we keep the risk level constant across all the
constraints.

Lastly, we also consider terminal constraints of the form
$\mathcal{X}_{F}:=\\{\boldsymbol{x}\in\mathbb{R}^{n_{x}}|F_{f}\boldsymbol{x}\leq
g_{f}\\},F_{f}\in\mathbb{R}^{v\times n_{x}},g_{f}\in\mathbb{R}^{v}$ and we
want to satisfy,

$\displaystyle\rho(f_{f,n}^{T}x(t+N|t)-g_{f,n})\leq\epsilon_{f},\quad\forall
n\in\mathbb{Z}_{1}^{v}$ (15)

where,
$F_{f}=\begin{bmatrix}f_{f,1}^{T}&f_{f,2}^{T}&\dotsc&f_{f,v}^{T}\end{bmatrix}^{T},g_{f}=\begin{bmatrix}g_{f,1}^{T}&g_{f,2}^{T}&\dotsc&g_{f,v}^{T}\end{bmatrix}^{T}$.
We write this constraint in shorthand as,
$\rho(\boldsymbol{x}_{N}\not\in\mathcal{X}_{F})\leq\epsilon_{f}$.

######  Remark 1.

The total number of risk constraints are $L+r+s+v$ for $L$ obstacles, $r$
state constraints, $s$ control constraints, and $v$ terminal constraints. With
some abuse of notation, we write the risk $\rho_{1-\alpha}$ to mean the
adjusted risk level $1-\alpha^{\prime}$ such that,

$1-\alpha^{\prime}=\frac{(1-\alpha)}{L+1}$

where $\alpha$ is the risk confidence level of the entire system and
$\alpha^{\prime}$ is the adjusted risk level for the risk constraints and cost
to attain the true confidence $\alpha$.

######  Assumption 3.

We assume that the measures of risk (used for safety, state, and control
constraints and the cost function) are coherent risk measures that can be
represented in their dual form as:

$\displaystyle\rho(X):=\sup_{Q\in\mathcal{Q}}E_{Q}(X),$

where, $\mathcal{Q}$ is a convex, closed set that we represent as
$\mathcal{Q}=\big{\\{}\boldsymbol{g}(q,\alpha)\leq
0,\sum_{j=1}^{J}p(j)q(j)=1,\,q(j)\geq 0,\,\forall j\in\mathcal{J}\big{\\}}$,
and $\boldsymbol{g}(q,\alpha)$ is a convex function in $q$. We assume that if
$\boldsymbol{g}(q,\alpha)$ is of dimension $>1$, all its elements constitute a
single function applied to all the components of $q$ separately. We also
assume that $\alpha\rightarrow 0$ corresponds to the risk-neutral case with
$\rho(X)\rightarrow\mathbb{E}(X)$ and $\alpha\rightarrow 1$ corresponds to
worst case (robust) with $\rho(X)\rightarrow\max\delta$.

In this work, we parameterize the control policy as an affine function of the
process noise, i.e., we solve for a disturbance feedback policy. In [32], the
authors reduced the number of decision variables during the computation of the
affine disturbance feedback policy and showed that for a linear, time-
invariant system, this policy is equivalent to using state feedback policies.
We apply this simplified affine disturbance feedback (SADF) as,

$\displaystyle u_{k}$
$\displaystyle=\sum_{m=0}^{k-1}K_{k-m}\delta_{m}+\eta_{k},$ (16a)
$\displaystyle\implies\boldsymbol{u}_{N}$
$\displaystyle=\boldsymbol{K}_{N}\boldsymbol{\delta}_{N}+\boldsymbol{\eta}_{N},$
(16b)

where, $u_{i}$ is an affine function of the disturbances, $\delta$, from time
$t$ to $t+i$, $K_{k-m},\eta_{k}$ are the decision variables in the MPC
optimization, and for a $N$ step (horizon) problem,

$\displaystyle\boldsymbol{K}_{N}$
$\displaystyle=\begin{bmatrix}0&\dotsc&\dotsc&0&0\\\ K_{1}&0&\dotsc&0&0\\\
\vdots&\ddots&\ddots&0&0\\\ K_{N-1}&K_{N-2}&\dotsc&K_{1}&0\end{bmatrix}$
$\displaystyle\boldsymbol{u}_{N}$
$\displaystyle=\begin{bmatrix}u_{0}&u_{1}&\dotsc&u_{N-1}\end{bmatrix}^{T}$
$\displaystyle\boldsymbol{\delta}_{N}$
$\displaystyle=\begin{bmatrix}{\delta}_{0}&{\delta}_{1}&\dotsc&{\delta}_{N-1}\end{bmatrix}^{T}$
$\displaystyle\boldsymbol{\eta}_{N}$
$\displaystyle=\begin{bmatrix}\eta_{0}&\eta_{1}&\dotsc&\eta_{N-1}\end{bmatrix}^{T}.$

We are now ready to present the problem we are interested in solving in this
paper.

######  Problem 1.

Consider the discrete-time dynamical system (7) and the randomly moving
obstacles $O_{l}$, $l\in\mathbb{Z}_{1}^{L}$, as defined in (8) and (9). Given
an initial condition $x_{0}\in\mathbb{R}^{n_{x}}$, a goal set
$\mathcal{X}_{f}\subset\mathbb{R}^{n_{x}}$, state constraints
$\mathcal{X}\subset\mathbb{R}^{n_{x}}$, control constraints
$\mathcal{U}\subset\mathbb{R}^{n_{u}}$, an immediate convex cost function
$r:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{u}}\to\mathbb{R}_{\geq 0}$, a
horizon $N\in\mathbb{N}_{\geq 0}$, and risk tolerances $\epsilon_{l}$,
$\epsilon_{x},\epsilon_{u},\epsilon_{F}$, for for obstacle, state, control and
terminal constraints respectively, compute the receding horizon controller
$\\{u_{k}\\}_{k=0}^{N-1}$ such that $x(t+N)\in\mathcal{X}_{f}$ while
satisfying the risk-sensitive safety constraints (12), i.e., the solution to
the following problem

$\displaystyle\begin{split}\min_{\boldsymbol{K}_{N},\boldsymbol{\eta}_{N}}\quad&J(x(t),\boldsymbol{u}):=\rho_{1-\alpha}\bigg{(}\sum_{k=0}^{N-1}(r(\boldsymbol{x}_{k},\boldsymbol{u}_{k})\bigg{)}\quad\end{split}$
(17a)
$\displaystyle\begin{split}\textrm{s.t.}\quad&\boldsymbol{x}_{k+1}=A\boldsymbol{x}_{k}+B\boldsymbol{u}_{k}+D{\delta}_{k},\end{split}$
(17b)
$\displaystyle\begin{split}&\boldsymbol{y}_{k}=C\boldsymbol{x}_{k},\end{split}$
(17c)
$\displaystyle\begin{split}&u_{k}=\sum_{m=0}^{k-1}K_{k-m}\delta_{m}+\eta_{i},\end{split}$
(17d)
$\displaystyle\begin{split}&\rho_{1-\alpha}(\boldsymbol{x}_{k+1}\not\in\mathcal{X})\leq\epsilon_{x},\end{split}$
(17e)
$\displaystyle\begin{split}&\rho_{1-\alpha}(\boldsymbol{u}_{k}\not\in\mathcal{U})\leq\epsilon_{u},\end{split}$
(17f)
$\displaystyle\begin{split}&\rho_{1-\alpha}\big{(}\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))\big{)}\leq\epsilon_{l},\forall
l\in\mathbb{Z}_{1}^{L},\end{split}$ (17g)
$\displaystyle\begin{split}\rho_{1-\alpha}(x_{N}\not\in\mathcal{X}_{F})\leq\epsilon_{f},\end{split}$
(17h)
$\displaystyle\begin{split}&\boldsymbol{x}_{0}=\boldsymbol{x}(t).\end{split}$
(17i)

Note that although the obstacles $\mathcal{O}_{l}$ are assumed to be convex
polytopes (8), the safe set $\mathcal{S}_{l}(t+k)$ in (10) is nonconvex in
$\boldsymbol{y}_{k}$. Hence, the minimum distance to $\mathcal{S}_{l}(t+k)$,
$\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))$, is also nonconvex in
$\boldsymbol{y}_{k}$. Therefore, the risk-sensitive safety constraint
(LABEL:eq:safetycon) is a nonconvex constraint in the decision variable $u$,
which renders optimization problem (17) nonconvex as well.

The next section will reformulate the state, control, and safety constraints
(LABEL:eq:stcon), (LABEL:eq:contcon), (LABEL:eq:safetycon) in order to obtain
a convex mixed-integer relaxation of (17), which yields locally optimal
solutions. Nonetheless, every such locally optimal solutions satisfies the
constraints of optimization (17) including the risk-sensitive safety
constraint (LABEL:eq:safetycon).

##  4 Risk-Constrained Receding Horizon Planning

This section breaks down the receding horizon control problem into several
parts. First, we modify the state and control constraints by finding efficient
approximations that rigorously satisfy the risk constraints. Next, we
specifically look at a tractable reformulation of the risk-obstacle avoidance
constraint. Note that the risk-averse state and control constraint tightening
can be computed offline because it depends only on the risk from the process
noise. On the other hand, the risk-averse obstacle avoidance constraint
depends on the distance of the system from the obstacle, which is constantly
varying and hence needs to be computed online. We then rewrite the non-convex
safe set as a set of mixed-integer constraints. We reformulate the terminal
constraint by adding discrete states such that we can reach the goal in
finite-time. Finally, we provide an efficient, tractable reformulation of the
risk cost function. Note that the proofs of the lemmas and propositions are
provided in the Appendix.

###  4.1 State and Control Constraint Tightening

######  Lemma 1 (Tightened state constraint).

Assuming the control policy (16a), a tightened set of state constraints,

$\displaystyle
f_{x,n}^{T}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}\big{)}$
$\displaystyle+\lVert
f_{x,n}^{T}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\rho(|{\delta}|)\leq\epsilon_{x}+g_{x,n},\,\forall
k\in\mathbb{Z}_{1}^{N},n\in\mathbb{Z}_{1}^{r}$ (18)

where,
$\boldsymbol{B}_{k}=\begin{bmatrix}A^{k-1}B&A^{k-2}B&\dotsc&B\end{bmatrix}$,
and
$\boldsymbol{D}_{k}=\begin{bmatrix}A^{k-1}D&A^{k-2}D&\dotsc&D\end{bmatrix}$,
guarantees that (LABEL:eq:stcon) holds.

The above tightening is useful because $\rho(|{\delta}|)$ is independent of
the disturbance feedback matrices. Hence, $\rho(|{\delta}|)$ can be computed
offline.

Similarly, input constraints of the form (LABEL:eq:contcon) are enforced as,

$\displaystyle f_{u,n}^{T}\boldsymbol{\eta_{i}}+\lVert
f_{u,n}^{T}\boldsymbol{K}_{k}\rVert_{1}\rho(|{\delta}|)\leq\epsilon_{u}+g_{u,n},$
$\displaystyle\quad k\in\mathbb{Z}_{0}^{N-1},n\in\mathbb{Z}_{1}^{s},$ (19)

and terminal constraints of the form (17h) are enforced as,

$\displaystyle
f_{f,n}^{T}\big{(}A^{N}x_{0}+\boldsymbol{B}_{N}\boldsymbol{\eta}_{N}\big{)}$
$\displaystyle+\lVert
f_{f,n}^{T}\big{(}\boldsymbol{B}_{N}\boldsymbol{K}_{N}+\boldsymbol{D}_{N}\big{)}\rVert_{1}\rho(|{\delta}|)\leq\epsilon_{f}+g_{f,n},\,\forall
n\in\mathbb{Z}_{1}^{v}.$ (20)

Now we reformulate the risk that arises from the moving obstacle
(LABEL:eq:safetycon). This safety constraint is given by,

${\rho}\big{[}\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))]=\rho\bigg{(}\min_{\boldsymbol{z}\in\mathcal{S}_{l}(t+k)}||\boldsymbol{y}(t+k|t)-\boldsymbol{z}||\bigg{)}\leq\epsilon_{l},\quad\forall
l\in\mathbb{Z}_{1}^{L}$ (21)

In (21), the safe set at time $t+k$, $\mathcal{S}_{l}(t+k)$, is a random
variable that is a function of the discrete measurement noise and the output,
$\boldsymbol{y}(t+k|k)$, is a random variable that is a function of the
process noise $(\delta_{0},\delta_{1},\dotsc,\delta_{k})$. Hence the distance
of the output $\boldsymbol{y}_{k}$ from the safe set is given by a random
variable, $\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))$, that has a joint
distribution of the measurement and process noise. This joint distribution has
a sample space of cardinality
$J=|\mathcal{D}|^{k}|\mathcal{J}|=(J_{\delta})^{k}J_{o}$ and a pmf given by
$\boldsymbol{p}\ =[p(1),p(2),\dotsc,p(J)]^{T}$.

######  Lemma 2 (Safety Constraint Reformulation).

If Assumptions 2 and 3 hold, then the L.H.S. of constraint
(LABEL:eq:safetycon) is equivalent to

$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu,h_{l,k}}$
$\displaystyle\lambda_{2}g^{*}\bigg{(}\lambda_{2}^{-1}\big{(}\boldsymbol{p}(h_{l,k}+\nu)+\boldsymbol{\lambda}_{1}\big{)}\bigg{)}-\nu$
(22) s.t. $\displaystyle\boldsymbol{\lambda}_{1}\succeq 0,\,\lambda_{2}\geq
0,$
$\displaystyle\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}\in\mathcal{S}_{l}^{j}(t+k),$
$\displaystyle\forall j\in\mathbb{Z}_{1}^{J},\exists
i\in\mathbb{Z}_{1}^{m_{l}},$

where,
$\boldsymbol{\lambda}_{1}\in\mathbb{R}^{J},\lambda_{2},\nu,h_{l,k}\in\mathbb{R}$
and $g^{*}$ is the convex conjugate [33] of the convex function $g$ that
describes the risk envelope of a coherent risk measure.

######  Remark 2.

Note that for simplicity, we are assuming that $g(q)\in\mathbb{R}$. The above
proof, however, can easily be extended to a vector-valued function
$\boldsymbol{g}$ of dimension $>1$ under Assumption 3. In this case,
$\boldsymbol{g}$ is a function $\bar{g}$ applied to each component of $q$,
i.e.,

$\boldsymbol{g}(q)=\begin{bmatrix}\bar{g}(q(1))&\bar{g}(q(2))&\dotsc&\bar{g}(q(J))\end{bmatrix}^{T}.$

The equivalent safety constraint reformulation is then given by,

$\displaystyle\min_{\boldsymbol{\lambda}_{1},\boldsymbol{\lambda_{2}},\nu,h_{l,k}}$
$\displaystyle{\sum_{j\in\mathbb{Z}_{1}^{J}}\big{\\{}\lambda^{j}_{2}\,\bar{g}^{*}\big{(}(\lambda_{2}^{j})^{-1}\big{(}{p(j)}(h_{l,k}+\nu)+\boldsymbol{\lambda}_{1}^{j}\big{)}\big{)}\big{\\}}-\nu}$
(23) s.t. $\displaystyle\boldsymbol{\lambda}_{1}\succeq
0,\,\boldsymbol{\lambda}_{2}\succeq 0,$
$\displaystyle\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}\in\mathcal{S}_{l}^{j}(t+k),~{}~{}\forall
j\in\mathbb{Z}_{1}^{J}$

Similar results could be obtained for g-entropic risk measures using the
Donsker-vardhan variational formula, as shown in [30].

The above reformulation applies to all coherent risk measures that satisfy
Assumption 3. Next, we present this formulation for a few specific risk
measures studied in our examples.

####  4.1.1 CVaR

For a CVaR constraint, the convex function,
$g(q)=\begin{bmatrix}q(1)-\frac{1}{1-\alpha}&\dotsc&q(J)-\frac{1}{1-\alpha}\end{bmatrix}^{T}$
is separated into $\bar{g}(q(j))=q(j)-\frac{1}{1-\alpha}$ $\forall
j\in\mathbb{Z}_{1}^{J}$. The convex conjugate
$\bar{g}^{*}(q^{*})=\frac{1}{1-\alpha}$ if $q^{*}=1$ and
$\bar{g}^{*}(q^{*})=+\infty$ otherwise can be applied to (23) and simplified
to get a linear program,

$\displaystyle\mathrm{CVaR}_{1-\alpha}(\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k)))=\mathrm{CVaR}_{1-\alpha}(h_{l,k}^{*})$
$\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\boldsymbol{\lambda_{2}},\nu}\sum_{j=1}^{J}\Bigg{\\{}\lambda^{j}_{2}\,\bar{g}^{*}\bigg{(}(\lambda_{2}^{j})^{-1}\big{(}{p(j)}(h_{l,k}^{j,*}+\nu)+\boldsymbol{\lambda}_{1}^{j}\big{)}\bigg{)}\Bigg{\\}}-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\boldsymbol{\lambda}_{2}\succeq 0$ $\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\boldsymbol{\lambda_{2}},\nu}\sum_{j=1}^{J}\frac{\lambda^{j}_{2}}{1-\alpha}-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\boldsymbol{\lambda}_{2}\succeq 0,$
$\displaystyle\quad\quad\quad\,{p(j)}(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j}=\lambda_{2}^{j},\quad\forall
j\in\mathbb{Z}_{1}^{J}.$

####  4.1.2 EVaR

For EVaR, the risk envelope constitutes
$g(q)=\sum_{j\in\mathbb{Z}_{1}^{J}}p(j)q(j)\ln(q(j))+\ln(1-\alpha)$ and,

$g^{*}(q^{*})=\sum_{j=1}^{J}p(j)\exp\Big{(}{\frac{q^{*}(j)-1}{p(j)}}\Big{)}-\ln(1-\alpha)$

We substitute the convex conjugate into (44) to obtain the following
exponential cone optimization,

$\displaystyle\mathrm{EVaR}_{1-\alpha}(\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k)))=\mathrm{EVaR}_{1-\alpha}(h_{l,k}^{*})$
$\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}\lambda_{2}g^{*}\bigg{(}\lambda_{2}^{-1}\big{(}\boldsymbol{p}(h_{l,k}^{j,*}+\nu)+\boldsymbol{\lambda}_{1}\big{)}\bigg{)}-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0$ $\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu,\boldsymbol{s}}\lambda_{2}\sum_{j=1}^{J}{p(j)\exp\Big{(}{\frac{\lambda_{2}^{-1}(p(j)(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j})}{p(j)}}\Big{)}-\lambda_{2}\ln(1-\alpha)-\nu}$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0$ $\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}\lambda_{2}\sum_{j=1}^{J}p(j)s(j)-\lambda_{2}\ln(1-\alpha)-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0,$
$\displaystyle\quad\quad\quad\big{(}s(j),\,\lambda_{2}p(j),\,p(j)(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j}\big{)}\in
K_{exp},\,\forall j\in\mathbb{Z}_{1}^{J}.$

####  4.1.3 Total variational distance ambiguity sets

The risk envelope for TVD is given by,
$g(q)=\sum_{j\in\mathbb{Z}_{1}^{J}}|q(j)p(j)-p(j)|-2\alpha$. When the
conjugate $g^{*}(q^{*})=\sum_{j\in\mathbb{Z}_{1}^{J}}q^{*}(j)+2\alpha$ when
$|q^{*}(j)/p(j)|\leq 1$ is substituted into (44), one obtains a linear
program,

$\displaystyle\mathrm{TVD}_{\alpha}(\zeta(\mathcal{S}_{l}(t+k)))=\mathrm{TVD}_{\alpha}(h_{l,k}^{*})$
$\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}\lambda_{2}g^{*}\bigg{(}\lambda_{2}^{-1}\big{(}\boldsymbol{p}(h_{l,k}^{j,*}+\nu)+\boldsymbol{\lambda}_{1}\big{)}\bigg{)}-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0$ $\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}\sum_{j=1}^{J}\big{(}p(j)(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j}\big{)}+2\lambda_{2}\alpha-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0$
$\displaystyle\qquad\quad|\lambda_{2}^{-1}\big{(}{p}(j)(h_{l,k}^{*}+\nu)+{\lambda}_{1}^{j}\big{)}|\leq
p(j),\quad\,\forall j\in\mathbb{Z}_{1}^{J}$ $\displaystyle=$
$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}\sum_{j=1}^{J}\big{(}p(j)(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j}\big{)}+2\lambda_{2}\alpha-\nu$
$\displaystyle\quad\text{s.t.}\quad\boldsymbol{\lambda}_{1}\succeq
0,\,\lambda_{2}\geq 0,$
$\displaystyle\qquad\quad-\lambda_{2}p(j)\leq{p}(j)(h_{l,k}^{j,*}+\nu)+{\lambda}_{1}^{j}\leq\lambda_{2}p(j),\quad\,\forall
j\in\mathbb{Z}_{1}^{J}.$

Now that we have seen how our general reformulation of the risk obtained in
Lemma 2 can be applied to various examples of coherent risk measures, we are
ready to present the MPC optimization that incorporates all the risk
constraint reformulations we have obtained so far.

######  Theorem 3.

Consider the MPC optimization given by (17) with confidence level $\alpha$ and
risk tolerances $\epsilon_{l}$, $l\in\mathbb{Z}_{1}^{L}$. If Assumption 1
holds, then (17) is equivalent to a minimization over
$\mathcal{V}=\\{\boldsymbol{K}_{N},\boldsymbol{\eta}_{N},\boldsymbol{\lambda}_{1},{\lambda_{2}},\nu,h_{l,k}\\}$
given by

$\displaystyle\begin{split}\min_{\mathcal{V}}\quad&J(\boldsymbol{x}(t),\boldsymbol{u}):=\rho\Bigg{(}\sum_{k=0}^{N-1}r(\boldsymbol{x}_{k},\boldsymbol{u}_{k})\Bigg{)}\end{split}$
(24a)
$\displaystyle\begin{split}\textrm{s.t.}\quad&\bigg{(}\lambda_{2}^{-1}\big{(}\boldsymbol{p}(h_{l,k}+\nu)+\boldsymbol{\lambda}_{1}\big{)}\bigg{)}-\nu\leq\epsilon_{l},\end{split}$
(24b) $\displaystyle\begin{split}&\boldsymbol{\lambda}_{1}\leq 0,\end{split}$
(24c) $\displaystyle\begin{split}&\lambda_{2}\geq 0\end{split}$ (24d)
$\displaystyle\begin{split}&\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}\in\mathcal{S}_{l}^{j}(t+k),\quad\forall
j\in\mathbb{Z}_{1}^{J}\end{split}$ (24e)
$\displaystyle\begin{split}(\ref{eq:dyn1}),(\ref{eq:dyn2}),(\ref{eq:ic}),(\ref{eq:stcon}).\end{split}$
(24f)

###### Proof.

We can substitute the result from Lemma 2 in (17) to get

$\displaystyle\begin{split}\min_{\boldsymbol{K}_{N},\boldsymbol{\eta}_{N}}\quad&J(\boldsymbol{x}(t),\boldsymbol{u}):=\sum_{k=0}^{N-1}r(\boldsymbol{x}_{k},\boldsymbol{u}_{k})\quad\end{split}$
(25a)
$\displaystyle\begin{split}\textrm{s.t.}\quad&(\ref{eq:dyn1}),(\ref{eq:dyn2}),(\ref{eq:ic}),(\ref{eq:stcon}),\end{split}$
(25b) $\displaystyle\begin{split}\eqref{eq:
safetycon_reformulate}\leq\epsilon_{l},\quad
l\in\mathbb{Z}_{1}^{L}.\end{split}$ (25c)

Suppose we have an optimal solution to (25) given by
$(\boldsymbol{K}_{N}^{*},\boldsymbol{\eta}_{N}^{*})$. As (25) is feasible, its
constraints must be satisfied; this implies the inner minimization (22) must
also be feasible, with solution
$(\boldsymbol{\lambda}_{1}^{*},{\lambda_{2}}^{*},\nu^{*},h_{l,k}^{*})$. Hence,
$(\boldsymbol{K}_{N}^{*},\boldsymbol{\eta}_{N}^{*},\boldsymbol{\lambda}_{1}^{*},{\lambda_{2}}^{*},\nu^{*},h_{l,k}^{*})$
must also be a feasible solution to (24) and yield the same objective value.
Conversely, denote the optimal solution to (24) as
$(\boldsymbol{K}_{N}^{*},\boldsymbol{\eta}_{N}^{*},\boldsymbol{\lambda}_{1}^{*},{\lambda_{2}}^{*},\nu^{*},h_{l,k}^{*})$.
The pair $(\boldsymbol{K}_{N}^{*},\boldsymbol{\eta}_{N}^{*})$ must be feasible
for (25) and gives the same objective value. Hence, the above optimization
(25) is equivalent to the one-layer optimization (24). ∎

###  4.2 Mixed-Integer Reformulation of the MPC optimization

This subsection frames the nonconvex safe set as a set of disjunctive
inequalities. These inequalities are incorporated in our optimization by
introducing a set of binary variables and using the Big-M reformulation [34].
The safe set (10) is defined as the region outside the obstacle $l$. Given
that an obstacle can rotate and translate by $R_{l}(t+k)$ and
$\boldsymbol{w}_{l}(t+k)$ from its nominal trajectory, we can write the safe
set at $t+k$ as the region outside $\mathcal{O}_{l}(t+k)$ described in (9). It
can equivalently be expressed as a result of the rotation and translation of
the nominal safe set itself

$\displaystyle\mathcal{S}_{l}(t+k)$
$\displaystyle=\mathbb{R}^{n_{y}}\backslash\mathcal{O}_{l}(t+k)$ (26)
$\displaystyle=R_{l}(t+k)\bar{\mathcal{S}_{l}}(t+k)+\boldsymbol{w}_{l}(t+k).$

In (LABEL:eq:safeset), $\mathcal{S}_{l}(t+k)$ is a nonconvex set. For some
obstacle $l\in\mathbb{Z}_{1}^{L}$, (LABEL:eq:safeset) can be rewritten as

$R_{l}(t+k)^{-1}\bigg{(}\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}-\boldsymbol{w}_{l}(t+k)\bigg{)}\in\bar{\mathcal{S}_{l}}(t+k).$

We know that $\boldsymbol{y}_{k}=C\boldsymbol{x}_{k}$ such that
$\boldsymbol{x}_{k}=A^{k}x_{0}+\boldsymbol{B}_{k}(\boldsymbol{\eta}_{k}+\boldsymbol{K}_{k}\boldsymbol{\delta}_{k}+\boldsymbol{D}_{k}\boldsymbol{\delta}_{k})$,
where
$\boldsymbol{\delta}_{k}=\begin{bmatrix}{\delta}_{1}&{\delta}_{2}&\dotsc{\delta}_{k}\end{bmatrix}^{T}$
is the process noise, i.e.,

$\displaystyle\small{R_{l}(t+k)^{-1}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}(\boldsymbol{\eta}_{k}+\boldsymbol{K}_{k}\boldsymbol{\delta}_{k})+\boldsymbol{D}_{k}{\delta}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}-\boldsymbol{w}_{l}(t+k)\big{)}\in\bar{\mathcal{S}_{l}}(t+k).}$

In the above equation, as described at the before Lemma 2, the safe set at
time $t+k$, $\mathcal{S}_{l}(t+k)$ is a random variable that is a function of
the discrete measurement noise and the output, $\boldsymbol{y}(t+k|k)$, that
in turn is a random variable that is a function of the process noise
$(\delta_{0},\delta_{1},\dotsc,\delta_{k})$. Hence, distance of the output
from the safe set, $h_{l,k}$, is a random variable that has a joint
distribution of the measurement and process noise. This joint distribution has
a sample space of cardinality
$J=|\mathcal{D}|^{k}|\mathcal{J}|=(J_{\delta})^{k}J_{o}$ and a pmf given by
$\boldsymbol{p}\ =[p(1),p(2),\dotsc,p(J)]^{T}$.

$\displaystyle\small{R_{l}^{j}(t+k)^{-1}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}+\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\boldsymbol{\delta}^{j}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}-\boldsymbol{w}_{l}^{j}(t+k)\big{)}\in\bar{\mathcal{S}_{l}}^{j}(t+k),}$

where $\\{R_{l}^{j}(t+k),\,{w}_{l}^{j}(t+k),\,\boldsymbol{\delta}^{j}_{k}\\}$
such that $j\in\mathbb{Z}_{1}^{J}$ is a realization of the measurement and
process noise from its joint distribution. Given that the obstacles are convex
polygons of the form (8), we write the safe region as the union of regions
outside of the halfspaces that define an obstacle as follows

$\bigvee_{i=1}^{m_{l}}\boldsymbol{c}_{i,l}^{T}\bigg{[}R_{l}^{j}(t+k)^{-1}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}+\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\boldsymbol{\delta}^{j}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}-\\\
\boldsymbol{w}_{l}^{j}(t+k)-\boldsymbol{a}_{l}(t+k)\big{)}+\boldsymbol{a}_{l}(t+k)\bigg{]}\geq
d_{i,l}.$ (27)

Because the above disjunctive inequalities, however, are hard to enforce, we
reformulate the constraint using a Big-M reformulation. The reformulation
converts the disjunctive inequalities into a set of constraints described
using binary variables, $\gamma_{i}^{j}\in\\{0,1\\}$ and a large positive
constant $M$. The value of $M$ depends on the bounds on $h_{l,k}^{j}$
(determined from the size of obstacle $l$) and $\boldsymbol{y}_{k}$ (dependent
on the state and control inputs). It can be computed using linear programming.
The Big-M reformulation of (27) is as follows

$\displaystyle\begin{split}\boldsymbol{c}_{i,l}^{T}\Bigg{[}&R_{l}^{j}(t+k)^{-1}\bigg{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}+\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\boldsymbol{\delta}^{j}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}-\\\
&\boldsymbol{w}_{l}^{j}(t+k)-\boldsymbol{a}_{l}(t+k)\bigg{)}+\boldsymbol{a}_{l}(t+k)\Bigg{]}\geq
d_{i,l}-M\gamma_{i}^{j},\end{split}$ (28a)
$\displaystyle\begin{split}\sum_{i=1}^{m_{l}}\gamma_{i}^{j}\leq m_{l}-1,\hskip
50.0pt\forall i\in\mathbb{Z}_{1}^{m_{l}},\,j\in\mathbb{Z}_{1}^{J}.\end{split}$
(28b)

Inequalities (28) provide output constraints that satisfy the risk-sensitive
obstacle avoidance constraint by taking into account measurement noise and
process noise. However, the cardinality of the joint distribution that
describes the distance from the obstacle, $h_{l,k}$, increases exponentially
with the horizon, $k$. With exponentially increasing mixed-integer variables,
the optimization soon becomes intractable.

To account for this, we introduce a new random variable, $\delta_{max,k}$,
whose cumulative distribution function is defined as follows

$\displaystyle\mathbb{P}(\delta_{max,k}\leq x):=$
$\displaystyle\mathbb{P}(|\delta_{0}|\leq x\,,\,|\delta_{2}|\leq
x\,,\,\dotsc\,,\,|\delta_{k-1}|\leq x)$ $\displaystyle=$
$\displaystyle\mathbb{P}(|\delta_{0}|\leq x)\mathbb{P}(|\delta_{2}|\leq
x)\dotsc\mathbb{P}(|\delta_{k-1}|\leq x)$ $\displaystyle=$
$\displaystyle\mathbb{P}(|\delta_{1}|\leq x)^{k}.$

We can find a conservative (inner) approximation of (28a) using,
${\delta}_{max,k}$, as follows,

$\displaystyle\boldsymbol{c}_{i,l}^{T}R_{l}(t+k)^{-1}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\boldsymbol{\delta}_{k}\geq$
$\displaystyle-\lVert\boldsymbol{c}_{i,l}^{T}R_{l}(t+k)^{-1}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\lVert\boldsymbol{\delta}_{k}\rVert_{\infty}$
$\displaystyle=$
$\displaystyle-\lVert\boldsymbol{c}_{i,l}^{T}R_{l}(t+k)^{-1}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\delta_{max,k}.$

Hence, we can rewrite (28a) as,

$\begin{split}-\big{\lVert}\boldsymbol{c}_{i,l}^{T}&R_{l}^{j}(t+k)^{-1}(\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k})\big{\rVert}_{1}\delta_{max,k}^{j}+\boldsymbol{c}_{i,l}^{T}\bigg{[}R_{l}^{j}(t+k)^{-1}\Big{(}A^{k}x_{0}+\\\
&\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}-\boldsymbol{w}_{l}^{j}(t+k)-\boldsymbol{a}_{l}\Big{)}+\boldsymbol{a}_{l}\bigg{]}\geq
d_{i,l}-M\gamma_{i}^{j}\end{split}$ (29)

Notice that in the above inequality, we have, with some abuse of notation,
reduced the cardinality of the joint distribution that describes, $h_{l,k}$,
from $J=|\mathcal{D}|^{k}|\mathcal{J}|=(J_{\delta})^{k}J_{o}$ to
$J=|\mathcal{D}||\mathcal{J}|=J_{\delta}J_{o}$. This means that the number of
constraints no longer increase exponentially with horizon length. Notice that
the approximation is not conservative at the beginning of the horizon, i.e.,
when $k=1$, the distribution described by $\delta_{max,k}$ is the same as
$\delta_{1}$.

###  4.3 Terminal constraints

In order to steer the system to the target region in finite time, we follow
the suggestion of [35] and define a new discrete state $\psi\in\\{0,1\\}$,
such that $\psi=0$ implies that the task has been completed at an earlier step
or at the current step and $\psi=1$ means that the task has not yet been
completed at the current time. The update equation of $\psi$ is then given by

$\psi_{k+1}=\psi_{k}-\mu_{k},$ (30)

where ${\mu}_{k}\in\\{0,1\\}$ is a discrete input.

Our goal to drive the system to a terminal set $\mathcal{X}_{F}$ is given by
the tightened state constraint (20). Additionally, we incorporate the
following constraints

$\displaystyle
f_{f,n}^{T}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}\big{)}$
$\displaystyle+\lVert
f_{f,n}^{T}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\rho(|{\delta}|)\leq\epsilon_{f}+g_{f,n}+\mathbbm{1}{M}(1-\mu_{k}),$
(31)

$\forall k\in\mathbb{Z}_{1}^{N},n\in\mathbb{Z}_{1}^{v}$, where
$\mathbbm{1}\in\mathbb{R}^{n_{x}}$ is a vector of $1$’s. Here, ${\mu}_{k}=1$
if the task of reaching the goal,
$\mathcal{X}_{F}=\\{\boldsymbol{x}\in\mathbb{R}^{n_{x}}|F_{f}\boldsymbol{x}\leq
g_{f}\\}$ is completed at time step $t+k+1$. Equation (30) implies that $\psi$
jumps from $1\rightarrow 0$, signaling completion of the task. After the task
completion, all other MPC problem constraints can be relaxed by adding the
term $M(1-\psi_{k})$ to them, i.e., any constraints of the form
$C_{1}\boldsymbol{s}_{k}+C_{2}\gamma_{i}+C_{3}\geq 0$ are modified to
$C_{1}\boldsymbol{s}_{k}+C_{2}\gamma_{i}+C_{3}+\mathbbm{1}M(1-\psi_{k})\geq
0,\,\forall i,k$ where
$\boldsymbol{s}_{k}=[\boldsymbol{K_{k},\eta_{k},\lambda_{1},\lambda_{2}},\nu,h_{l,k}]$.
We also add the following terminal constraint at time $t+N$ to ensure that the
task is completed

$\psi_{N}=0.$ (32)

Note that the discrete state $\psi$ need not be a binary variable as long as
we enforce the constraint,

$0\leq\psi_{k}\leq 1,\quad k=1,2,\ldots,N.$ (33)

The MPC objective function is then modified as

$\displaystyle\min_{\mathcal{V}}\quad J(\mathcal{V}):=$
$\displaystyle\,\rho\bigg{(}\sum_{k=0}^{N-1}\big{(}r(\boldsymbol{u}_{k})+\psi_{k}\big{)}\bigg{)}$
(34) $\displaystyle=$
$\displaystyle\,\rho\bigg{(}\sum_{k=0}^{N-1}r\big{(}\sum_{m=0}^{k-1}K_{k-m}\delta_{m}+\eta_{k}\big{)}\bigg{)}+\sum_{k=0}^{N-1}\psi_{k}$

where
$\mathcal{V}=\\{\boldsymbol{K_{N},\eta_{N},\lambda_{1},\lambda_{2}},\nu,h_{l,k}\\}$
and $r(\boldsymbol{u}_{k})$ is a convex function of the control input with
$r(0)=0$.

###  4.4 MPC Objective

For the MPC cost (34), consider the case, $r(\boldsymbol{u})=\lVert
R\boldsymbol{u}\rVert_{1}$, where $R\in\mathbb{R}^{n_{u}}$,

$\displaystyle J(t)$ $\displaystyle=\rho\Big{(}\sum_{k=0}^{N-1}\lVert
R\boldsymbol{u}_{k}\rVert_{1}\Big{)}+\sum_{k=0}^{N-1}\psi_{k},$ (35)
$\displaystyle=\rho\bigg{(}\sum_{k=0}^{N-1}\big{\lVert}\sum_{m=0}^{k-1}RK_{k-m}\delta_{m}+R\eta_{k}\big{\rVert}_{1}\bigg{)}+\sum_{k=0}^{N-1}\psi_{k}.$

This subsection introduces two methods to compute the control effort risk
given by $\rho\big{(}\sum_{k=0}^{N-1}\,r(\boldsymbol{u_{k}})\big{)}$. The
first method provides an exact value of the risk and the second method
provides an approximation. The first method loosely follows the steps taken to
calculate the moving obstacle risk (see Lemma 2) and will be more
computationally expensive because the control effort $r(\boldsymbol{u}_{k})$
is a joint distribution of $(\delta_{0},\dotsc,\delta_{N-1})$ that grows with
the horizon length $N$. The second method will utilize the constraint
tightening tools used in Lemma 1 to approximate the value of the control
effort risk. This approximation will be more computationally efficient. The
examples in Section 5 contrast the two methods.

####  4.4.1 Exact computation of control effort risk

We define the control effort as a random variable,
$Z:=\sum_{k=0}^{N-1}\big{\lVert}\sum_{m=0}^{k-1}K_{k-m}\delta_{m}+\eta_{k}\big{\rVert}_{1}$.
The sample space of $Z$ consists of the joint probability distribution of
$(\delta_{0},\dotsc,\delta_{N-1})$, which has cardinality $|\mathcal{D}|^{N}$.
All the realizations of $Z$ can be vectorized as
$\boldsymbol{z}=\begin{bmatrix}z(1),z(2),\dotsc,z(|\mathcal{D}|^{N})\end{bmatrix}$.
Note that
$z(j)=\sum_{k=0}^{N-1}\big{\lVert}\sum_{m=0}^{k-1}K_{k-m}\delta_{m}^{j}+\eta_{k}\big{\rVert}_{1},~{}\forall
j\in\mathbb{Z}_{1}^{|\mathcal{D}|^{N}}$, where $\delta_{m}^{j}$ is a
realization of $\delta_{m}$ from the joint pmf. If the pmf is denoted by
$\boldsymbol{p}_{\Delta}\in\mathbb{R}^{|\mathcal{D}|^{N}}$, then,

$\displaystyle\rho\bigg{(}\sum_{k=0}^{N-1}\big{\lVert}\sum_{m=0}^{k-1}RK_{k-m}\delta_{m}+R\eta_{k}\big{\rVert}_{1}\bigg{)}$
(36)
$\displaystyle=\left\\{\begin{matrix}\max_{q(1),\dotsc,q(|\mathcal{D}|^{N})}&\mathbb{E}_{Q}\big{[}Z\big{]}&\\\
\text{s.t.}&{g}(q)\leq 0&\\\ &-q(j)\leq 0&\forall
j\in\mathbb{Z}_{1}^{|\mathcal{D}|^{N}}\\\
&\sum_{j=1}^{J}p_{\Delta}(j)q(j)=1&\end{matrix}\right.$
$\displaystyle=\left\\{\begin{matrix}\min_{\boldsymbol{\xi}_{1},\xi_{2},\vartheta}\quad&\xi_{2}g^{*}\bigg{(}\xi_{2}^{-1}\big{(}\boldsymbol{p}_{\Delta}(\boldsymbol{z}+\vartheta)-\boldsymbol{\xi}_{1}\big{)}\bigg{)}-\vartheta&\\\
\text{s.t.}\quad&\boldsymbol{\xi}_{1}\leq 0&\\\ &\xi_{2}\geq
0&,\end{matrix}\right.$

where,
$\boldsymbol{\xi}_{1}\in\mathbb{R}^{|\mathcal{D}|^{N}},\,\boldsymbol{\xi}_{2},\vartheta\in\mathbb{R}$
are the dual variables, see Lemma 2 for details on how we find the dual
function and obtain the conjugate in the above minimization. Note that the
number of constraints grow exponentially with the horizon length.

####  4.4.2 Approximation of control effort risk

We can alternatively approximate the cost as,

$\displaystyle\rho\bigg{(}\sum_{k=0}^{N-1}\big{\lVert}\sum_{m=0}^{k-1}RK_{k-m}\delta_{m}+R\eta_{k}\big{\rVert}_{1}\bigg{)}$
(37)
$\displaystyle\leq\sum_{k=0}^{N-1}\Big{(}\rho\big{(}\lVert\sum_{m=0}^{k-1}(RK_{k-m}\delta_{m})\rVert_{1}\big{)}+\lVert
R\eta_{k}\rVert_{1}\Big{)}$ (Subadditivity)
$\displaystyle\leq\sum_{k=0}^{N-1}\Big{(}\big{\lVert}\sum_{m=0}^{k-1}RK_{k-m}\big{\rVert}_{1}\rho\big{(}|\delta|\big{)}+\lVert
R\eta_{k}\rVert_{1}\Big{)}$ $\displaystyle{\text{(i.i.d disturbances)}},$

where we obtained the first inequality by using the subadditivity of norms and
then the translational invariance property of coherent risk measures. The
second inequality results from observing that all disturbances are i.i.d and
can be replaced by $\delta$. We use the homogeneity of norms and coherent risk
measures to obtain the final result (similar to Lemma 1).

The above cost approximation eliminates thee additional $|\mathcal{D}|^{N}$
constraints that result from (36). This approximation deprioritizes task
completion; i.e., when we substitute this upper bound into (35), the term
$\sum_{k=0}^{N-1}\psi_{k}$ has less weight compared to when we use the exact
cost (36). Another approximation of the true cost would be using the random
variable $\delta_{max,N}$ as seen in (29). In using an approximation of
$(\delta_{0},\dotsc,\delta_{N-1})$ via $\delta_{max,N}$, we reduce the number
of constraints from $|\mathcal{D}|^{N}$ to $|\mathcal{D}|$.

###  4.5 Properties of MPC

We now combine all the parts of the MPC into one optimization.

$\displaystyle\min_{\mathcal{V}}$
$\displaystyle\sum_{k=0}^{N-1}\Big{(}\big{\lVert}\sum_{m=0}^{k-1}RK_{k-m}\big{\rVert}_{1}\rho\big{(}|\delta|\big{)}+\lVert
R\eta_{k}\rVert_{1}\Big{)}$ (38)
$\displaystyle\sum_{j=1}^{J}\Bigg{\\{}\lambda^{j}_{2}\,\bar{g}^{*}\bigg{(}(\lambda_{2}^{j})^{-1}\big{(}{p(j)}(h_{l,k}^{j}+\nu)+\boldsymbol{\lambda}_{1}^{j}\big{)}\bigg{)}\Bigg{\\}}-\nu\leq\epsilon_{l}+M_{k}$
$\displaystyle\text{L.H.S}~{}\eqref{eq: bigM_max_approx}\geq
d_{i,l}-M\gamma_{i}^{j}-M_{k}$ $\displaystyle
F_{x}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}\big{)}+\lVert
F_{x}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\rho(|{\delta}|)\leq\epsilon_{x}+g_{x}+M_{k}$
$\displaystyle F_{u}\boldsymbol{\eta_{k}}+\lVert
F_{u}\boldsymbol{K}_{k}\rVert_{1}\rho(|\boldsymbol{\delta}|)\big{)}\leq\epsilon_{u}+g_{u}+M_{k}$
$\displaystyle\eqref{eq:taskCompletion},~{}\eqref{eq:invariance_states},~{}\eqref{eq:terminalConst},~{}\eqref{eq:discreteStateConst}.$

where $M_{k}=M(1-\psi_{k})$ and
$\mathcal{V}=\\{\boldsymbol{K}_{N},\boldsymbol{\eta}_{N},\boldsymbol{\lambda}_{1},\boldsymbol{\lambda}_{2},\nu,h_{l,k}\\}$.
The constraints must hold $\forall
k\in\mathbb{Z}_{1}^{N-1},\,l\in\mathbb{Z}_{1}^{L},\,j\in\mathbb{Z}_{1}^{J},$
and $i\in\mathbb{Z}_{1}^{m_{l}}$. The solution to the deterministic MPC
problem (38) is also a solution to (17). The convex, mixed-integer relaxation
of a nonconvex optimization problem in (38) results in locally optimal
solutions.

######  Proposition 1 (Risk-sensitive recursive feasibility).

If the optimization (38) is feasible at time $t$, then it is feasible at time
$t+1$ with confidence $\alpha$.

######  Remark 3.

Proposition 1 provides a loose bound on the infeasibility of the MPC. In the
case of CVaR, TVD, and EVaR, we know that the risk measures are upper bounds
for VaR (see Figure 1) and can hence provide tighter bounds on the likelihood
of infeasibility. Recall from Remark 1 that the confidence level is adjusted
to account for multiple risk constraints. Hence, we can quantify the bounds on
probability of MPC infeasibility in terms of the confidence $\alpha$,

$\displaystyle\mathbb{P}\\{\text{MPC infeasible}\\}$
$\displaystyle\leq\mathbb{P}\\{\delta_{0}>\rho(|\delta|)\,\cup\,h_{l,0}>\rho(h_{l,0})\\}$
$\displaystyle\leq\mathbb{P}\\{\delta_{0}\geq\rho(|\delta|)\\}+\sum_{l=1}^{L}\mathbb{P}\\{h_{l,0}\geq\rho(h_{l,0})\\}$
$\displaystyle\leq\frac{1-\alpha}{L+1}+L\frac{1-\alpha}{L+1}$ (VaR probability
bound) $\displaystyle\leq 1-\alpha.$

MPC is often used to plan local trajectories given a reference trajectory or a
set of waypoints from a higher-level global planner like A* or RRT [36, 25].
Let $\\{\boldsymbol{w}_{1},\boldsymbol{w}_{2},\dotsc,\boldsymbol{w}_{K}\\}$ be
a given a sequence of waypoints. We call a waypoint $\boldsymbol{w}_{j+1}$
$N$-step reachable from $\boldsymbol{w}_{j}$, if there exists a feasible
solution to (38) with $\boldsymbol{x}_{0}=\boldsymbol{w}_{j}$ and
$\boldsymbol{x}_{K}=\boldsymbol{w}_{j+1}$.111We assume that we obtain these
waypoints from a higher-level planner like A* or RRT. Analyzing the $N$-step
reachability of the waypoints is out of the scope of this paper and we
consider it an avenue of future work.

Algorithm 1 Follow waypoints

Number of waypoints visited, $W=0$

while $W<K$ do

Initialize $(\boldsymbol{x}_{0},\psi_{0})=(\boldsymbol{w}_{W},1)$

Set desired goal $\boldsymbol{x}_{des}=\boldsymbol{w}_{W+1}$

while $\psi_{0}\neq 0$ do

Solve (38) to obtain policy
${\\{(0,\eta_{0}^{*}),\dotsc,({K}_{N-1}^{*},\eta_{N-1}^{*})\\}}$

Update $\boldsymbol{x}_{0}=A\boldsymbol{x}_{0}+B\eta_{0}^{*}+D\delta_{0}$

Update $\psi_{0}=\psi_{0}-\mu_{0}$

if $\boldsymbol{x}_{0}=\boldsymbol{x}_{des}$ then

$W=W+1$

end if

end while

end while

######  Proposition 2 (Finite-time task completion).

Assuming that the waypoint $\boldsymbol{w}_{j+1}$ is $N$-step reachable from
$\boldsymbol{w}_{j},\,\forall j\in\mathbb{Z}_{1}^{K-1}$, Algorithm 1 gives a
sequence of control inputs to move from $\boldsymbol{w}_{0}$ to
$\boldsymbol{w}_{K}$ in finite time with confidence $\alpha$.

##  5 Numerical Results

To illustrate the effectiveness of this method, we present numerical
experiments that were run on MATLAB using the YALMIP toolbox [37] with a
Gurobi solver [38] (for CVaR and TVD) and a Mosek solver [39] (for EVaR) .

###  5.1 Simple 2D system

We first look at the two-dimensional discrete system
$x_{k+1}=Ax_{k}+Bu_{k}+D\delta_{k}$ that is similar to the example we
considered in [26], but with process noise.

${\small A=\begin{bmatrix}1.0475&-0.0463\\\
0.0463&0.9690\end{bmatrix},\,B=\begin{bmatrix}0.028\\\
-0.0195\end{bmatrix},\,D=\begin{bmatrix}0.028\\\ -0.0195\end{bmatrix}.}$

The process noise can take values,
$\delta_{k}\in\begin{bmatrix}-0.2&-0.1&0&0.1&0.2\end{bmatrix}.\\\ $ The
control constraints are

$-100\leq u_{k}\leq 100.$

One randomly moving obstacle interferes with the MPC solution path that would
be found in the absence of any obstacles. The obstacle rotates either $0$ or
$\pi/4$ degrees. The obstacle can translate along the x-axis by
$0,-0.25,\text{or }0.25$ m. The horizon length is $N=8$.

We compare the disturbance feedback policies obtained by using three different
risk measures - CVaR, EVaR, and TVD by comparing the total cost of reaching
the goal, the percentage of infeasible optimizations, and the average
computation time for each MPC iteration.

Cost Comparison: A fair comparison of using the exact cost versus the over
approximation of the cost can be made only if the constraints of the MPC (38)
remain the same. For 50 Monte Carlo simulations, we compare the MPC
trajectories obtained when using CVaR risk. We compare the trajectory costs
resulting from using a) the exact cost as computed in (36) and b) the upper
bound of the exact cost (37), see Table 1. The average time taken for each MPC
iteration is also provided. We see that the time taken for each MPC iteration
is significantly higher when the exact CVaR cost is used. The control effort
is also higher when using the exact cost. This is because the over-
approximated cost always penalizes higher control effort more than task
completion (control is parameterized as an affine function of the
disturbance). Figure 3 shows us the qualitative difference between the 50
Monte Carlo simulations when $\alpha=0.9$. We emphasize that task completion
is prioritized when the exact CVaR cost is used.

Figure 3: Comparison of the trajectories obtained using the exact cost (36) and the upper bound cost (37). The gray rectangles show possible obstacle configurations. The darker rectangle has a higher probability of occurrence and the lighter rectangle has a lower probability of occurrence. $\alpha$ | Average cost ($\times 10^{3}$)
---|---
| Exact | Upper bound
0.1 | 4.44 | 3.94
0.4 | 4.51 | 3.94
0.8 | 5.36 | 3.93
0.9 | 6.20 | 3.89
Time(s) | 83.68 | 6.32
Table 1: Average trajectory cost for CVaR MPC using different costs

Feasibility comparison: For different risk levels, $\alpha$, we also compare
the number of times the MPC optimization is infeasible when we use CVaR, EVaR,
and TVD risk measures, with the cost (37). The results for 50 Monte Carlo
simulations are summarized in Table 2. It has been shown that
$\text{VaR}\leq\text{CVaR}\leq\text{EVaR}$ and $\text{CVaR}\leq\text{TVD}$,
see Section 2. Proposition 1 provides us a loose bound for the probability of
infeasibility of the MPC. Table 2 shows us that as $\alpha$ increases
(increasing conservativeness), the percentage of infeasible optimizations
decreases. Furthermore, the actual likelihood of infeasibility is much lower
than the bounds obtained in Proposition 1. Thus, the bounds of Proposition 1
are verified in this case, though the degree of tightness is unknown.

$\alpha$ | MPC infeasible (%)
---|---
| CVaR | EVaR | TVD
0.1 | 5.3 | 1.64 | 0
0.4 | 5.9 | 1.09 | 0
0.8 | 6.4 | 0.18 | 0
0.9 | 2.7 | 0 | 0
Time (s) | 6.32 | 42.33 | 3.61
Table 2: Results for infeasibility of risk-aware MPC

###  5.2 Quadcopter

Figure 4: A comparison of the TVD MPC trajectories with the expectation-based
MPC ($\alpha\rightarrow 0$) trajectory. On the right, the shaded blue region
is zoomed in from a different perspective to illustrate the behavior near one
of the random realizations of the obstacle.

We consider a quadcopter that must follow given waypoints while avoiding
randomly moving obstacles and adhering to state and control constraints,
Algorithm 1. The quadcopter is described by the states
$(x,y,z,\phi,\theta,\varphi,\dot{x},\dot{y},\dot{z},\dot{\phi},\dot{\theta},\dot{\varphi})^{T}$.
The position of the quadcopter in 3D space is given by the coordinates $x,y,z$
and the roll, pitch, and yaw are given by $\phi,\theta,\varphi$ respectively.
The model of the quadcopter is given by

$\ddot{x}=-g\theta,\,\ddot{y}=g\theta,\,\ddot{z}=-\frac{u_{1}}{m}-g,\,$
$\ddot{\phi}=\frac{u_{2}}{I_{xx}},\,\ddot{\theta}=\frac{u_{3}}{I_{yy}},\,\ddot{\varphi}=\frac{u_{4}}{I_{zz}},$

where $m$ is the quadcopter’s mass, $g$ is the acceleration due to gravity,
and $I_{xx},I_{yy},I_{zz}$ are the quadcopter moments of inertia about the
$x,y,z$-axes of the system. The control inputs are given by
$u_{1},u_{2},u_{3},u_{4}$. We used the following parameters: $m=0.65$kg,
$l=0.23$m, $I_{xx}=0.0075$kg.m2, $I_{yy}=0.0075$kg.m2, $I_{zz}=0.0013$kg.m2,
$g=9.81$m.s-2 [26, 25].

The TVD risk constraint has two parameters: the confidence level, $\alpha$,
and the risk-threshold, $\epsilon$. We chose
$\alpha\in\\{0.8,0.99\\},\,\epsilon=0.04$. The waypoints are given by regions
of size $[-0.5,0.5]^{3}$ around the waypoint center (denoted by o in Figure
4). We chose a horizon length of $N=15$ for the MPC optimization. We
considered the case of having one randomly translating and rotating obstacle.
The obstacle is a rectangular box of size $2$x$1$x$4$ m3; it can rotate by
$\frac{\pi}{2}$ and translate by $4$m along the y-axis and $1$m along the
z-axis. Figure 4 shows all the different configurations of this obstacle at
different times. We further allow for process noise
$\delta\in\\{-0.2,-0.1,0,0.1,0.2\\}$. The continuous-time system is
discretized with a time-interval of $0.02$ sec. As the risk-averseness of the
system grows, the trajectory followed by the quadcopter maintains a greater
distance from all possible configurations of the moving obstacle.

##  6 Conclusions

This work proposed a risk-aware framework for motion planning with obstacle
avoidance using MPC. We provided reformulations of the risk constraint and the
cost to get a tractable, convex, mixed-integer optimization. We also provided
guarantees on the recursive feasibility and the finite-time task completion of
the MPC. Coherent risk measures make the system distributionally robust to
disturbances and this provides stronger guarantees on system safety compared
to expectation-based MPC (or stochastic MPC). This risk-aware formulation is a
generalization of stochastic MPC and robust MPC into one framework through the
adjustment of the risk-level $\alpha$.

There are several ways in which this formulation can be improved. As seen in
the numerical results, the time taken for each MPC optimization, is a few
seconds. This does not allow for real-time application of the MPC. The MPC can
be sped-up by removing (or reducing the number of) the mixed-integer
constraints. For the obstacle avoidance constraint, one can find half-space
approximations of the obstacle [40, 13]. The formulation also accounts for
only discrete probability distributions. An extension to continuous
distributions will require approximation methods like Sample Average
Approximation [41]. Furthermore, coherent risk measures provide distributional
robusteness in the expectation of the cost. A natural extension is to
incorporate distributionally robust chance constraints in the MPC [42].
Lastly, this work is limited to linear, discrete-time systems. Future work
involves accounting for nonlinear dynamics and providing continuous-time
safety guarantees in a risk-aware manner [43].

## Acknowledgement

The authors would like to thank Skylar Wei and Thomas Touma for helping run
many simulations. The authors would also like to acknowledge valuable input
from Prithvi Akella, Skylar Wei, and Siddharth Nair.

## References

  * [1] A. Bouman, M. F. Ginting, N. Alatur, M. Palieri, D. D. Fan, T. Touma, T. Pailevanian, S.-K. Kim, K. Otsu, J. Burdick, et al., Autonomous Spot: Long-Range Autonomous Exploration of Extreme Environments with Legged Locomotion, arXiv preprint arXiv:2010.09259.
  * [2] D. D. Fan, K. Otsu, Y. Kubo, A. Dixit, J. Burdick, A.-A. Agha-Mohammadi, Step: Stochastic traversability evaluation and planning for risk-aware off-road navigation, in: Robotics: Science and Systems, RSS Foundation, 2021, pp. 1–21.
  * [3] S. Daftry, N. Abcouwer, T. D. Sesto, S. Venkatraman, J. Song, L. Igel, A. Byon, U. Rosolia, Y. Yue, M. Ono, Mlnav: Learning to safely navigate on martian terrains, IEEE Robotics and Automation Letters 7 (2) (2022) 5461–5468.  doi:10.1109/LRA.2022.3156654.
  * [4] K. Nagatani, S. Kiribayashi, Y. Okada, K. Otake, K. Yoshida, S. Tadokoro, T. Nishimura, T. Yoshida, E. Koyanagi, M. Fukushima, et al., Emergency response to the nuclear accident at the fukushima daiichi nuclear power plants using mobile rescue robots, Journal of Field Robotics 30 (1) (2013) 44–63.
  * [5] E. Seraj, M. Gombolay, Coordinated control of uavs for human-centered active sensing of wildfires, in: 2020 American Control Conference (ACC), IEEE, 2020, pp. 1845–1852.
  * [6] U. Rosolia, A. Carvalho, F. Borrelli, Autonomous racing using learning model predictive control, in: 2017 American Control Conference (ACC), 2017, pp. 5115–5120.  doi:10.23919/ACC.2017.7963748.
  * [7] U. Rosolia, A. Singletary, A. D. Ames, Unified multi-rate control: from low level actuation to high level planning, arXiv preprint arXiv:2012.06558.
  * [8] A. D. Ames, X. Xu, J. W. Grizzle, P. Tabuada, Control barrier function based quadratic programs for safety critical systems, IEEE Transactions on Automatic Control 62 (8) (2017) 3861–3876.  doi:10.1109/TAC.2016.2638961.
  * [9] L. Blackmore, M. Ono, B. C. Williams, Chance-constrained optimal path planning with obstacles, IEEE Transactions on Robotics 27 (6) (2011) 1080–1094.  doi:10.1109/TRO.2011.2161160.
  * [10] A. M. Jasour, B. C. Williams, Risk contours map for risk bounded motion planning under perception uncertainties, Robotics: Science and Systems.
  * [11] S.-K. Kim, A. Bouman, G. Salhotra, D. D. Fan, K. Otsu, J. Burdick, A.-a. Agha-mohammadi, Plgrim: Hierarchical value learning for large-scale exploration in unknown environments, in: Proceedings of the International Conference on Automated Planning and Scheduling, Vol. 31, 2021, pp. 652–662.
  * [12] F. Borrelli, Constrained optimal control of linear and hybrid systems, Vol. 290, Springer, 2003.
  * [13] S. H. Nair, V. Govindarajan, T. Lin, C. Meissen, H. E. Tseng, F. Borrelli, Stochastic mpc with multi-modal predictions for traffic intersections, arXiv preprint arXiv:2109.09792.
  * [14] A. Bemporad, M. Morari, Robust model predictive control: A survey, in: Robustness in identification and control, Springer, 1999, pp. 207–226.
  * [15] A. Mesbah, Stochastic model predictive control: An overview and perspectives for future research, IEEE Control Systems Magazine 36 (6) (2016) 30–44.  doi:10.1109/MCS.2016.2602087.
  * [16] A. Jasour, Risk aware and robust nonlinear planning (rarnop), Course Notes for MIT 16 (2019) S498.
  * [17] M. Ono, M. Pavone, Y. Kuwata, J. Balaram, Chance-constrained dynamic programming with application to risk-aware robotic space exploration, Autonomous Robots 39 (4) (2015) 555–571.
  * [18] W. Han, A. Jasour, B. Williams, Non-gaussian risk bounded trajectory optimization for stochastic nonlinear systems in uncertain environments, arXiv preprint arXiv:2203.03038.
  * [19] S. Koenig, R. G. Simmons, Risk-sensitive planning with probabilistic decision graphs, in: Principles of Knowledge Representation and Reasoning, Elsevier, 1994, pp. 363–373.
  * [20] H. Xu, S. Mannor, Distributionally robust Markov decision processes, in: Advances in Neural Information Processing Systems, 2010, pp. 2505–2513.
  * [21] Y. Chen, J. Kim, J. Anderson, Distributionally robust decision making leveraging conditional distributions, arXiv preprint arXiv:2204.00138.
  * [22] A. Majumdar, M. Pavone, How should a robot assess risk? towards an axiomatic theory of risk in robotics, in: Robotics Research, Springer, 2020, pp. 75–84.
  * [23] P. Artzner, F. Delbaen, J.-M. Eber, D. Heath, Coherent measures of risk, Mathematical finance 9 (3) (1999) 203–228.
  * [24] S. Singh, Y. Chow, A. Majumdar, M. Pavone, A framework for time-consistent, risk-sensitive model predictive control: Theory and algorithms, IEEE Transactions on Automatic Control.
  * [25] A. Hakobyan, G. C. Kim, I. Yang, Risk-aware motion planning and control using cvar-constrained optimization, IEEE Robotics and Automation Letters 4 (4) (2019) 3924–3931.
  * [26] A. Dixit, M. Ahmadi, J. W. Burdick, Risk-sensitive motion planning using entropic value-at-risk, in: European Control Conference, 2021.
  * [27] M. Ahmadi, X. Xiong, A. D. Ames, Risk-averse control via CVaR barrier functions: Application to bipedal robot locomotion, IEEE Control Systems Letters 6 (2021) 878–883.
  * [28] P. Sopasakis, M. Schuurmans, P. Patrinos, Risk-averse risk-constrained optimal control, in: 2019 18th European Control Conference (ECC), 2019, pp. 375–380.  doi:10.23919/ECC.2019.8796021.
  * [29] Y. Chen, U. Rosolia, W. Ubellacker, N. Csomay-Shanklin, A. D. Ames, Interactive multi-modal motion planning with branch model predictive control, IEEE Robotics and Automation Letters 7 (2) (2022) 5365–5372.
  * [30] A. Ahmadi-Javid, Entropic value-at-risk: A new coherent risk measure, Journal of Optimization Theory and Applications 155 (3) (2012) 1105–1123.
  * [31] A. Shapiro, Distributionally robust stochastic programming, SIAM Journal on Optimization 27 (4) (2017) 2258–2275.
  * [32] J. Zhang, T. Ohtsuka, Stochastic model predictive control using simplified affine disturbance feedback for chance-constrained systems, IEEE Control Systems Letters 5 (5) (2021) 1633–1638.  doi:10.1109/LCSYS.2020.3042085.
  * [33] D. Bertsekas, Convex optimization theory, Vol. 1, Athena Scientific, 2009.
  * [34] A. Vecchietti, S. Lee, I. Grossman, Modeling of Discrete/Continuous Optimization Problems: Characterization and Formulation of Disjunctions and Their Relaxations, Computers and Chemical Engineering 27 (3) (2003) 433–448.
  * [35] A. Richards, J. P. How, Model predictive control of vehicle maneuvers with guaranteed completion time and robust feasibility, in: Proceedings of the 2003 American Control Conference, 2003., Vol. 5, 2003, pp. 4034–4040 vol.5.  doi:10.1109/ACC.2003.1240467.
  * [36] B. T. Lopez, J. P. How, Aggressive collision avoidance with limited field-of-view sensing, in: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017, pp. 1358–1365.  doi:10.1109/IROS.2017.8202314.
  * [37] J. Lofberg, Yalmip: A toolbox for modeling and optimization in matlab, in: Computer Aided Control Systems Design, 2004 IEEE International Symposium on, IEEE, 2004, pp. 284–289.
  * [38] Gurobi Optimization, LLC, Gurobi Optimizer Ref. Manual (2022).
URL https://www.gurobi.com

  * [39] M. ApS, The MOSEK optimization toolbox for MATLAB manual. Version 9.0. (2019).
URL http://docs.mosek.com/9.0/toolbox/index.html

  * [40] S. X. Wei, A. Dixit, S. Tomar, J. W. Burdick, Moving obstacle avoidance: a data-driven risk-aware approach, arXiv preprint arXiv:2203.14913.
  * [41] S. Kim, R. Pasupathy, S. G. Henderson, A guide to sample average approximation, Handbook of simulation optimization (2015) 207–243.
  * [42] A. Dixit, M. Ahmadi, J. W. Burdick, Distributionally robust model predictive control with total variation distance, arXiv preprint arXiv:2203.12062.
  * [43] A. Jasour, W. Han, B. Williams, Convex risk bounded continuous-time trajectory planning in uncertain nonconvex environments, Robotics: Science and Systems.
  * [44] S. Boyd, L. Vandenberghe, Convex Optimization, Cambridge University Press, USA, 2004\.

##  Appendix A Proof of Lemma 1

###### Proof.

First, rewrite the risk state constraint (15) as,

$\displaystyle\rho(f_{x,n}^{T}(A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{u}_{k}+\boldsymbol{D}_{k}\boldsymbol{\delta}_{k})-g_{x,n})\leq\epsilon_{x},\quad\forall
k\in\mathbb{Z}_{1}^{N},n\in\mathbb{Z}_{1}^{r}$

where,
$\boldsymbol{B}_{k}=\begin{bmatrix}A^{k-1}B&A^{k-2}B&\dotsc&B\end{bmatrix}$,
and
$\boldsymbol{D}_{k}=\begin{bmatrix}A^{k-1}D&A^{k-2}D&\dotsc&D\end{bmatrix}$.
We replace $\boldsymbol{u}_{k}$ with the SADF control policy (16a),

$\displaystyle\epsilon_{x}$
$\displaystyle\geq\rho\big{(}f_{x,n}^{T}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}(\boldsymbol{\eta}_{k}+\boldsymbol{K}_{k}\boldsymbol{\delta}_{k})+\boldsymbol{D}_{k}\boldsymbol{\delta}_{k}\big{)}-g_{x,n}\big{)}$
(39)
$\displaystyle=f_{x,n}^{T}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}\big{)}+\rho\big{(}f_{x,n}^{T}(\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k})\boldsymbol{\delta}_{k}\big{)}-g_{x,n}$
$\displaystyle=f_{x,n}^{T}\big{(}A^{k}x_{0}+\boldsymbol{B}_{k}\boldsymbol{\eta}_{k}\big{)}+\rho\big{(}f_{x,n}^{T}\sum_{m=0}^{k-1}\big{(}A^{k-m-1}(BK_{k-m}+D)\big{)}{\delta}_{m}\big{)}-g_{x,n}$

The second term on the right-hand side of the above inequality can be
simplified by using the subadditivity, monotonicity, and positive homogeneity
properties of coherent risk measures and then i.i.d. nature of the
disturbances respectively,

$\displaystyle\rho\bigg{(}f_{x,n}^{T}\sum_{m=0}^{k-1}\big{(}A^{k-m-1}(BK_{k-m}+D)\big{)}{\delta}_{m}\bigg{)}$
$\displaystyle\leq\sum_{m=0}^{k-1}\rho\bigg{(}f_{x,n}^{T}\big{(}A^{k-m-1}(BK_{k-m}+D)\big{)}{\delta}_{m}\bigg{)}$
(Subadditivity)
$\displaystyle\leq\sum_{m=0}^{k-1}\rho\bigg{(}|f_{x,n}^{T}\big{(}A^{k-m-1}(BK_{k-m}+D)\big{)}|\,|{\delta}_{m}|\bigg{)}$
(Monotonicity)
$\displaystyle\leq\sum_{m=0}^{k-1}|f_{x,n}^{T}\big{(}A^{k-m-1}(BK_{k-m}+D)\big{)}|\,\rho(|{\delta}_{m}|)$
(Positive Homogeneity) $\displaystyle\leq\lVert
f_{x,n}^{T}\big{(}\boldsymbol{B}_{k}\boldsymbol{K}_{k}+\boldsymbol{D}_{k}\big{)}\rVert_{1}\rho(|{\delta}|).$
(i.i.d disturbances)

Hence, satisfying the tightened constraint (18) implies satisfaction of the
state constraint (15). ∎

##  Appendix B Proof of Lemma 2

###### Proof.

To find the distance of $\boldsymbol{y}_{k}$ from the safe set,
$\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))$, we define a set of variables
$h_{l,k}^{j}\geq 0$, $l\in\mathbb{Z}_{1}^{L}$ and $k=0,\ldots,N-1$ satisfying

$\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}=\boldsymbol{z}$
(40)

$\forall j\in\mathbb{Z}_{1}^{J},\forall k\in\mathbb{Z}_{0}^{N-1}$ and for some
$i\in\mathbb{Z}_{1}^{m_{l}}$, which is the distance from each
$\boldsymbol{y}_{k}$ to a point $\boldsymbol{z}\in\mathcal{X}$. If
$\boldsymbol{z}\in\mathcal{S}_{l}^{j}(t+k)$, then minimizing $h_{l,k}$ defines
the line segment connecting $\boldsymbol{y}_{k}$ and $\boldsymbol{z}$, which
is the minimum distance to the set $\mathcal{S}_{l}^{j}(t+k)$. Therefore,

$\begin{split}\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))&=\min_{\boldsymbol{z}\in\mathcal{S}_{l}(t+k)}||\boldsymbol{y}(t+k)-\boldsymbol{z}||\\\
&=\left\\{\begin{matrix}\min_{h_{l,k}}&h_{l,k}\qquad\\\
\text{s.t.}&\boldsymbol{y}_{k}+\frac{\boldsymbol{c}_{i,l}}{||\boldsymbol{c}_{i,l}||}h_{l,k}^{j}\in\mathcal{S}_{l}^{j}(t+k),\end{matrix}\right.\end{split}$
(41)

and we denote $h_{l,k}^{*}$ as the solution to (41).

Next, substitute the dual form of coherent risk measures (given by the
representation theorem) from (4) into the L.H.S. of (LABEL:eq:safetycon).
Then, replace the risk envelope $\mathcal{Q}$ with the convex representation
given in Assumption 3. That is,
$\rho(\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k)))=\max_{Q\in\mathcal{Q}}\mathbb{E}_{Q}\big{[}\zeta(\boldsymbol{y}_{k},\mathcal{S}_{l}(t+k))\big{]}=\max_{Q\in\mathcal{Q}}\mathbb{E}_{Q}\big{[}h_{l,k}^{*}\big{]}$,
where (41) is used.

$\displaystyle\max_{q(1),\dotsc,q(J)}$
$\displaystyle\mathbb{E}_{Q}\big{[}h_{l,k}^{*}\big{]}$ (42) s.t.
$\displaystyle{g}(q)\leq 0,\,-q(j)\leq 0,$ $\displaystyle\forall
j\in\mathbb{Z}_{1}^{J},$
$\displaystyle\sum_{j\in\mathbb{Z}_{1}^{J}}p(j)q(j)=1.$

The dual of this problem is given by,

$\displaystyle\min_{\boldsymbol{\lambda}_{1},{\lambda}_{2},\nu}\max_{\boldsymbol{q}}$
$\displaystyle{\big{\\{}\sum_{j\in\mathbb{Z}_{1}^{J}}\big{[}q(j)p(j)h_{l,k}^{*}+\lambda^{j}_{1}q(j)+\nu
p(j)q(j)\big{]}-{\lambda}_{2}{g}(q)-\nu\big{\\}}}$ (43) s.t.
$\displaystyle\boldsymbol{\lambda}_{1}\succeq 0,\,{\lambda}_{2}\geq 0,$

where,
$\boldsymbol{\lambda}_{1}=\begin{bmatrix}\lambda^{1}_{1},\dotsc,\lambda^{J}_{1}\end{bmatrix}\in\mathbb{R}^{J},\,{\lambda}_{2}\text{
and }\nu\in\mathbb{R}$ are the dual variables. We conclude that (43) and (42)
are equivalent because strong duality holds by Slater’s condition [44].
Slater’s condition is satisfied by showing strict feasibility for (42), i.e.,
there exists a feasible solution to (42) such that the inequality constraints
hold with strict inequalities. One such solution occurs when $q(j)=1,\,\forall
j\in\mathbb{Z}_{1}^{J}$. We can find the maximum value of the Lagrangian in
the objective of (43) when we know the exact form of the function $g$.

We can equivalently write the inner maximization of (43) in the form of the
convex conjugate of $g$ given by $g*$

$\displaystyle\min_{\boldsymbol{\lambda}_{1},\lambda_{2},\nu}$
$\displaystyle\lambda_{2}g^{*}\bigg{(}\lambda_{2}^{-1}\big{(}\boldsymbol{p}(h_{l,k}^{*}+\nu)+\boldsymbol{\lambda}_{1}\big{)}\bigg{)}-\nu$
(44) s.t. $\displaystyle\boldsymbol{\lambda}_{1}\succeq 0,\,\lambda_{2}\geq
0.$

The above minimization is convex in the dual variables because the perspective
operation preserves convexity [44]. We know from the conjugacy theorem ([33],
Proposition 1.6.1) that properness of $g$ implies properness of $g^{*}$.
Finally, substituting minimization problem (41) for $h^{*}_{l,k}$ in
optimization (44) gives (22). ∎

######  Remark 4.

The term $\boldsymbol{p}(h_{l,k}^{*}+\nu)$ in (44) is just the expected value,
i.e., $\boldsymbol{p}(h_{l,k}^{*}+\nu)=\mathbb{E}_{P}(h_{l,k}^{*}+\nu)$.

##  Appendix C Proof of Proposition 1

###### Proof.

Assume that the feasible solution to (38) at time $t$ is given by the control
policy
$\\{(0,\eta_{0}^{*}),({K}_{1}^{*},\eta_{1}^{*}),\dotsc,({K}_{N-1}^{*},\eta_{N-1}^{*})\\}$.
At time $t$, we apply the control input $u_{0}=\eta_{0}^{*}$. Since (18), (19)
hold for all $\delta_{0}\leq\rho(|\delta|)$, if $\delta_{0}>\rho(|\delta|)$,
the state and control constraints may not hold and the MPC optimization may no
longer be feasible. Similarly, if the distance to the obstacle is greater than
the risk of the distance, the MPC may no longer be feasible, i.e., if
$h_{l,0}>\rho(h_{l,0})$.

Let us assume for simplicity that
$\delta_{0}\leq\rho(|\delta|),\,h_{l,0}\leq\rho(h_{l,0})$ hold at time $t$.
The optimization is feasible at time $t+1$ if there exists a feasible input at
time $t+N$ that does not violate constraints. Since $\psi_{N}^{*}=0$ by virtue
of the terminal constraint, all the constraints in the optimization are
relaxed thereafter. Note that the state $\psi_{N}=0$ is invariant due to (30)
and (33) and that $\mu_{k}=0$ at all time after the task has been completed.
Therefore, once the optimization constraints are relaxed they will remain this
way. A control input $(K_{N},\eta_{N})=(0,0)$ ensures that the optimization is
feasible. At time $t+1$, a feasible solution to (38) is given by the control
sequence
$\\{(0,{K}_{1}^{*}\delta_{0}+\eta_{1}^{*}),\dotsc,({K}_{N-1}^{*},\eta_{N-1}^{*}),(0,0)\\}$.

Finally, we aim to quantify the probability of the constraints at time $t+1$
no longer being satisfied by the control input $u_{0}=\eta_{0}^{*}$.

$\displaystyle\mathbb{P}\\{$ $\displaystyle\text{MPC infeasible}\\}$
$\displaystyle\leq\mathbb{P}\\{\delta_{0}>\rho(|\delta|)\,\cup\,h_{l,0}>\rho(h_{l,0})\\}$
$\displaystyle\leq\mathbb{P}\\{\delta_{0}\geq\rho(|\delta|)\\}+\mathbb{P}\\{h_{l,0}\geq\rho(h_{l,0})\\}$
$\displaystyle=\mathbb{P}\\{\delta_{0}-\mathbb{E}(|\delta|)\geq\rho(|\delta|)-\mathbb{E}(|\delta|)\\}+\mathbb{P}\\{h_{l,0}-\mathbb{E}(h_{l,0})\geq\rho(h_{l,0})-\mathbb{E}(h_{l,0})\\}$
(Subtracting $\mathbb{E}(|\delta|),\,\mathbb{E}(h_{l,0})$ from both sides)
$\displaystyle\leq\frac{\sigma_{\delta}^{2}}{\sigma_{\delta}^{2}+\big{(}\rho(|\delta|)-\mathbb{E}(|\delta|)\big{)}^{2}}+\frac{\sigma_{h}^{2}}{\sigma_{h}^{2}+\big{(}\rho(h_{l,0})-\mathbb{E}(h_{l,0})\big{)}^{2}}{\text{(Cantelli's
inequality)}}$

where, $\sigma_{\delta}^{2},\,\sigma_{h}^{2}$ are the variances of $\delta$
and $h_{l,0}$ respectively. Note that as $\alpha$ increases, the risk gets
larger as a greater value of $\alpha$ corresponds to a more risk-averse
setting. Hence, the upper bound on $\mathbb{P}\\{\text{MPC infeasible}\\}$
gets smaller.

We know that when $\alpha\rightarrow 0$ (risk-neutral),
$\rho(|\delta|)\rightarrow\mathbb{E}(|\delta|),\,\rho(h_{l,0})\rightarrow\mathbb{E}(h_{l,0})$,
and $\mathbb{P}\\{\text{MPC infeasible}\\}\leq 1$. Similarly,
$\alpha\rightarrow 1$ (risk-averse),
$\rho(|\delta|)\rightarrow\max|\delta|,\,\rho(h_{l,0})\rightarrow\max
h_{l,0}$, and $\mathbb{P}\\{\text{MPC infeasible}\\}\rightarrow 0$ (because
$\mathbb{P}\\{\delta_{0}>\rho(|\delta|)\,\cup\,h_{l,0}>\rho(h_{l,0})\\}\rightarrow
0$). Hence, (38) is feasible at time $t+1$ if it is feasible at time $t$ with
increasing probability as the confidence level $\alpha$ increases. ∎

##  Appendix D Proof of Proposition 2

###### Proof.

(Adapted from [26]) Consider the simple case of starting from
$\boldsymbol{w}_{0}$ and reaching $\boldsymbol{w}_{1}$, i.e., when we have
exactly two waypoints. We implement Algorithm 1 till $\psi_{0}=0$. Let
$J_{t}^{*}$ be the cost of the MPC optimization (38) at time $t$. The feasible
solution to (38) at $t$ is given by the input sequence
$\\{(0,\eta_{0}^{*}),({K}_{1}^{*},\eta_{1}^{*}),\dotsc,({K}_{N-1}^{*},\eta_{N-1}^{*})\\}$
and the state sequence $\\{\psi_{0}^{*},\psi_{1}^{*},\dotsc,\psi_{K}^{*}\\}$.
At time $t+1$, with confidence $\alpha$, the cost of the MPC optimization is
$J_{t+1}^{*}\leq J_{t}^{*}-\lVert R\eta_{0}\rVert_{1}-\psi_{0}^{*}.$ This is
true because we know from Proposition 1 that, with confidence $\alpha$, at
time $t+1$,
$\\{(0,{K}_{1}^{*}\delta_{0}+\eta_{1}^{*}),\dotsc,({K}_{N-1}^{*},\eta_{N-1}^{*}),(0,0)\\}$
is a feasible control input with $\psi(t+K|t+1)=0$, i.e., $J_{t}^{*}$ will
incur no additional cost from $(K_{N},\eta_{N}|t+1)=(0,0)$ and
$\psi(t+K+1|t+1)=0$. Since $J_{t+1}^{*}-J_{t}^{*}\leq-\lVert
R\eta_{0}\rVert_{1}-\psi_{0}^{*}$, the cost decreases by at least $1$ at each
time step till the task is completed. Considering that $J_{t}^{*}$ is always
positive and finite, it will take a finite number of steps to reach
$J_{k}^{*}=0,\,k\geq t$. Hence, the policy to take the system from
$\boldsymbol{w}_{0}$ to $\boldsymbol{w}_{1}$ will be implemented in finite
time, with confidence $\alpha$. By induction, the system will reach
$\boldsymbol{w}_{K}$ from $\boldsymbol{w}_{0}$ in finite time with confidence
$\alpha$. ∎