# Control of Uncertain PWA Systems using Difference-of-Convex Decompositions

Siddharth H. Nair, Yvonne R. Stürz Email IDs:{siddharth$\\_$nair,
<EMAIL_ADDRESS>
###### Abstract

In this work, we analyse and design feedback policies for discrete-time
Piecewise-Affine (PWA) systems with uncertainty in both the affine dynamics
and the polytopic partition. The main idea is to utilise the Difference-of-
Convex (DC) decomposition of continuous PWA systems to derive quadratic
Lyapunov functions as stability certificates and stabilizing affine policies
in a higher dimensional space. When projected back to the state space, we
obtain time-varying PWQ Lyapunov functions and time-varying PWA feedback
policies.

##  I Introduction

A discrete-time Piecewise-Affine (PWA) system is described by the dynamics

$\displaystyle x(t+1)$ $\displaystyle=f(x(t),u(t))$ (1)
$\displaystyle=A_{i}x(t)+B_{i}u(t)+c_{i},~{}~{}\forall(x(t),u(t))\in\Omega_{i}$

where $x(t),u(t)$ are the state and input respectively at time $t$, and
$\Omega_{i}$, $i=1,\dots,n_{r}$ form a polytopic partition of the state and
input space $\Omega=\mathcal{X}\times\mathcal{U}\subset\mathbb{R}^{n+m}$,
denoted as $\cup\Omega_{i}=\Omega$. PWA systems are a versatile modeling
framework; They can approximate smooth nonlinear dynamics arbitrarily well
([1]) and have also been shown to be equivalent to certain classes of hybrid
systems such as Mixed Logical Dynamical (MLD) systems and Linear
Complementarity (LC) systems under mild conditions ([2]). More recently in
[3], it was shown that continuous PWA systems can be written as the difference
of two continuous convex PWA functions and that they can be written as
optimizing processes.

The analysis and control of PWA systems has been widely studied in the
literature. In [4, 5, 6], stabilizing feedback controllers are synthesized
using quadratic and Piecewise-Quadratic (PWQ) Lyapunov functions as stability
certificates. PWQ Lyapunov functions, being a generalisation of quadratic
Lyapunov functions, provide less conservative stability certificates but are
more difficult to synthesize. They require an explicit description of the
polytopic partition $\sqcup\Omega_{i}=\Omega$ and need to account for all
possible transitions between the polytopic pieces $\Omega_{i}$ explicitly.
Similarly, the works [7, 8] consider piecewise-linear and piecewise-polynomial
Lyapunov functions for stability. A significantly more challenging problem is
the synthesis of optimal feedback control of PWA systems, studied in [9, 10,
11]. Here, the control problem is complicated by the fact that the stabilizing
controller must transition between polytopic parititions in an optimal fashion
over a finite-time horizon. In this article, we focus on the simpler task of
synthesizing stabilizing feedback control but for uncertain PWA systems.

A PWA system is said to be uncertain if the dynamics function $f(\cdot)$ is
not known exactly. Uncertain PWA systems are relatively less studied in the
literature due to the complexity of uncertain switching systems. The
uncertainty descriptions considered in the literature assume some form of
parametric uncertainty in the matrices $A_{i}$, $B_{i}$, $c_{i}$ but with
known, static partition $\cup\Omega_{i}=\Omega$ ([12, 13, 14]). This
uncertainty description is justified when the PWA framework is used to
approximate a smooth nonlinear system. However for a system that is inherently
hybrid and uncertain, the polytopic partition $\cup\Omega_{i}(t)=\Omega$
itself may be dynamic with time-varying polytopic pieces $\Omega_{i}(t)$.

In this article, we investigate the control of uncertain PWA systems where
both the affine dynamics and the polytopic partition are uncertain leveraging
results in the control and analysis of nominal PWA systems from [15, 16, 17].
The main challenge in the synthesis of stabilizing control of such systems is
the derivation of a stability certificate that does not require an explicit
description of the uncertain, time-varying polytopic partition
$\cup\Omega_{i}(t)=\Omega$. An obvious solution to this problem would be to
construct a common quadratic Lyapunov function on the state space but this has
been shown to be overly conservative ([4]). We show how this issue can be
addressed for continuous PWA systems for which a Difference of Convex (DC)
functions decomposition can be derived ([3]). Inspired by techniques in [16,
17], we show that the DC decomposition permits a construction of affine
stabilizing policies with quadratic Lyapunov functions in a higher dimensional
space, which when projected back to the state space result in time-varying PWA
policies and time-varying PWQ Lyapunov functions.

The remainder of the article is organized as follows. We briefly discuss our
notations in Section II before formally stating our problem statement in
Section III. Section IV presents the key technique we use for expressing the
PWA dynamics in a higher dimensional space. Section V describes our synthesis
procedure in detail which we demonstrate on two applications in Section VI.

##  II Notation

$\|\cdot\|$ denotes the Euclidean norm in $\mathbb{R}^{n}$ unless explicitly
stated otherwise. With slight abuse of notation, $\|\cdot\|$ also denotes the
norm on $\mathbb{R}^{n\times n}$ induced by the Euclidean norm.
$I_{n}\in\mathbb{R}^{n\times n}$ is the identity matrix and $\mathbf{0}$ is a
matrix of zeros with relevant dimensions. $[x]_{i}$ denotes the $i$th element
of vector $x\in\mathbb{R}^{n}$, $[M]_{ij}$ denotes the $i$th element in the
$j$th column of matrix $M\in\mathbb{R}^{n\times m}$ and $[M]_{i}$ denotes its
$i$th column. We denote the Hermitian of a real square matrix $M$ as
$\text{He}(M)=\frac{M+M^{\top}}{2}$. Given matrices
$A_{1},\dots,A_{N}\in\mathbb{R}^{n\times m}$, we denote their convex hull as
the set $\text{conv}\\{A_{k}\\}_{k=1}^{N}\subset\mathbb{R}^{n\times m}$. Given
matrices $A_{1},\dots A_{N}\in\mathbb{R}^{n\times n}$, we denote by
$\text{blkdiag}(A_{1},\dots,A_{N})\in\mathbb{R}^{Nn\times Nn}$ as the matrix
with block diagonals given by $A_{1},\dots,A_{N}$ and zeros everywhere else.
As a shorthand, we write the symmetric block matrix $\begin{bmatrix}A&B\\\
B^{\top}&C\end{bmatrix}$ as $\begin{bmatrix}A&B\\\ \star&C\end{bmatrix}$.

##  III Problem Formulation

We start with the Difference of Convex (DC) decomposition of $f(\cdot)$ in
(1). As shown in [3], all continuous PWA functions defined on convex polytopes
admit a DC decomposition and so we restrict our focus to such systems.

###### Assumption 1

The PWA system dynamics $f(\cdot)$ in (1) is continuous
$\forall(x(t),u(t))\in\Omega$ where $\Omega=\mathcal{X}\times\mathcal{U}$ is a
convex set with polyhedral partition $\Omega=\cup\Omega_{i}$.

Consider the DC decomposition of the dynamics (1) given by

$\displaystyle x(t+1)$ $\displaystyle=Ax(t)+\gamma(x(t))-\eta(x(t))+Bu(t),$
(2) $\displaystyle[\gamma(x)]_{i}$ $\displaystyle=\max_{1\leq j\leq\alpha}\
[E_{j}]_{i}x+[d_{j}]_{i},$ $\displaystyle[\eta(x)]_{i}$
$\displaystyle=\max_{1\leq l\leq\beta}\
[H_{l}]_{i}x+[f_{l}]_{i},~{}~{}i=1,\dots,n$

where without loss of generality, we have assumed that the convex functions
$\gamma(\cdot),\eta(\cdot)$ are functions of state $x(t)$ only and the number
of affine functions $\alpha$ and $\beta$ for $\gamma(\cdot)$ and $\eta(\cdot)$
respectively are the same for each dimension $i=1,\dots,n$. This DC
decomposition can be obtained using the constructive procedure outlined in
Lemma 4 of [3].

###### Remark 1

If $\gamma(\cdot),\eta(\cdot)$ are functions of $u$, we augment the state
space as $\tilde{x}=[x^{\top}u^{\top}]$ by adding integrators.

###### Remark 2

If $[\gamma(x)]_{i}=\max_{1\leq j\leq\alpha_{i}}\ [E_{j}]_{i}x+[d_{j}]_{i}$,
define $\alpha=\max_{1\leq i\leq n}\alpha_{i}$ and add $\alpha-\alpha_{i}$
affine functions that lower bound the convex function $[\gamma(x)]_{i}$ on the
set $\mathcal{X}$ to get $[\gamma(x)]_{i}=\max_{1\leq j\leq\alpha}\
[E_{j}]_{i}x+[d_{j}]_{i}$. A similar procedure can be repeated for $\eta(x)$.

Suppose that the DC system in (2) is uncertain, i.e., the matrices
$E_{j},d_{j},H_{j},f_{j}$ are uncertain and time-varying, but bounded. We
formalize this in the assumption below.

###### Assumption 2

The matrix-valued random variable
$\mathbf{W}(t)\in\mathbb{R}^{n\times(n+1)(\alpha+\beta)}$ given by

$\displaystyle\mathbf{W}(t)=[E_{1}(t)\ d_{1}(t)\dots E_{\alpha}(t)\
d_{\alpha}(t)\ H_{1}(t)\ f_{1}(t)$ $\displaystyle\dots H_{\beta}(t)\
f_{\beta}(t)]$

is independent, identically distributed (i.i.d) for all time $t\geq 0$ and
supported on a known compact convex polytope
$\mathcal{W}=\text{conv}\\{W^{k}\\}_{k=1}^{n_{V}}\subset\mathbb{R}^{n\times(n+1)(\alpha+\beta)}$
where $W^{k}=[E^{k}_{1}\ d^{k}_{1}\dots E^{k}_{\alpha}\ d^{k}_{\alpha}\
H^{k}_{1}\ f^{k}_{1}\dots H^{k}_{\beta}\ f^{k}_{\beta}]$.

To emphasize that system (2) is time-varying, we write the uncertain convex
functions as $\gamma(t,x(t)),\eta(t,x(t))$. This system description captures
the scenario of PWA systems with uncertainty in both the PWA function
definitions ($A_{i},B_{i},c_{i}$ for (1)) and the polytopic partitions
($\Omega_{i}$ for (1)).

We also assume that the origin $x=\mathbf{0}$ is an unforced ($u=\mathbf{0}$)
equilibrium for (2). More formally, we assume the following matching
condition.

###### Assumption 3

The origin is an unforced equilibrium of system (2), i.e.,

$\gamma(t,\mathbf{0})=\eta(t,\mathbf{0})\
\forall\mathbf{W}(t)\in\mathcal{W},t\geq 0\Leftrightarrow\max_{1\leq
j\leq\alpha}[d_{j}^{k}]_{i}=\max_{1\leq l\leq\beta}[f_{l}^{k}]_{i}$

for all $i=1,\dots,n$ and $k=1,\dots,n_{V}$.

In this article, we aim to synthesize PWA feedback policies

$\displaystyle u(t,x)=F_{i}(t)x+g_{i}(t)\ \forall
x\in\tilde{\mathcal{X}}_{i}(t)$ (3)

with disjoint partition $\tilde{\mathcal{X}}_{i}(t)$, s.t.
$\cup\tilde{\mathcal{X}}_{i}(t)=\mathcal{X}$, such that the origin is
exponentially stable for the closed-loop system. Notice that the partition on
which the policy is defined is time-varying. This class of uncertain PWA
systems has not been studied in the literature to the best of our knowledge.
Most approaches for feedback control synthesis for PWA systems in the
literature rely on an explicit description of the static polytopic partitions
and construct affine policies local to each partition. This is impossible for
the PWA systems considered in this work because the polytopic partitions are
dynamic (time-varying) and unknown. Therefore, to deal with these uncertain
hybrid systems, we present a technique that allows us to design PWA policies
without explicitly computing the polytopic partitions. We illustrate our
approach on two uncertain PWA systems in Section VI: A) an inverted pendulum
against a soft wall and B) a robot collaborating with a human to transport a
payload.

##  IV Lifted Quadratic Representations for Convex Piecewise Affine Functions

In this section, we describe the key technique for verifying if a continuous
PWA function satisfies a given quadratic inequality without explicitly
computing the polytopic partitions. First, consider the element-wise convex
PWA function $\lambda:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$,

$\lambda(x)=\max_{1\leq j\leq\xi}\\{E_{j}x+d_{j}\\}$

where the $\max\\{\cdot\\}$ operation is defined element-wise. Recursively
define the following functions

$\displaystyle\lambda_{j}(x)=\max\\{\lambda_{j-1}(x),E_{j}x+d_{j}\\}\quad
j=2,\dots,\xi$ (4)

with $\lambda_{1}(x)=E_{1}x+d_{1}$. Note that due to the fact that the
$\max\\{\cdot\\}$ operation can be equivalently written in a nested fashion
(for e.g., $\max\\{p,q,r\\}=\max\\{\max\\{p,q\\},r\\}$), we actually have
$\lambda_{\xi}(x)=\lambda(x)$.

Each function $\lambda_{j}(x)$ defined in (4) can be equivalently written as
the unique solution to $2n$ affine inequalities and $1$ quadratic equality in
$(x,\lambda_{j-1}(x))$ as shown below.

$\displaystyle\lambda_{1}(x)=E_{1}x+d_{1},$
$\displaystyle\lambda_{j}(x)\geq\lambda_{j-1}(x),\ \lambda_{j}(x)\geq
E_{j}x+d_{j},$ (5)
$\displaystyle(\lambda_{j}(x)-\lambda_{j-1}(x))^{\top}(\lambda_{j}(x)-E_{j}x-d_{j})=0,\
j=2,\dots,\xi.$

Given any $x\in\mathbb{R}^{n}$, the functions $\lambda_{j}(x)~{}\forall
j=1,\dots,\xi$ are the unique solutions to (IV). Consequently, the function
$\lambda(x)$ (or equivalently, its graph $(x,\lambda(x))$) is given by the
solution to a set of affine inequalities and quadratic equalities given in
(IV) in the space
$(x,\lambda_{1},\dots,\lambda_{\alpha})\in\mathbb{R}^{n(\xi+1)}$ where
$x,\lambda_{1},\dots,\lambda_{\alpha}$ are arbitrary variables. The next
proposition offers a useful tool to construct a convex relaxation of the set
of solutions $(x,\lambda(x))$ given by (IV).

###### Proposition 1

Let

$\chi(x)=[1\ x^{\top}\ \lambda_{1}(x)^{\top}\
\lambda_{2}(x)^{\top}\dots\lambda_{\xi}(x)^{\top}]^{\top}$

and define the selection matrices
$S_{j}\in\mathbb{R}^{(1+3n)\times(n(\xi+1)+1)}$ such that $S_{j}\chi(x)=[1\
x^{\top}\ \lambda_{j-1}(x)^{\top}\ \lambda_{j}(x)^{\top}]^{\top}$ for
$j=2,\dots,\xi$ and $S_{1}\in\mathbb{R}^{(1+2n)\times(n(\xi+1)+1)}$ such that
$S_{1}\chi(x)=[1\ x^{\top}\ \lambda_{1}(x)^{\top}]^{\top}$. Given any diagonal
matrices $D_{j}\in\mathbb{R}^{n\times n}$ and any non-negative matrices
$M_{j}\in\mathbb{R^{+}}^{n\times n}$ $~{}\forall j=1,\dots,\xi$, define the
set

$\displaystyle\mathfrak{X}(x)=\\{\chi$ $\displaystyle=[1\ x^{\top}\
\lambda_{1}^{\top}\dots\lambda_{\xi}^{\top}]^{\top}\in\mathbb{R}^{n(\xi+1)+1}:$
$\displaystyle\chi^{\top}S_{j}^{\top}G_{j}^{\top}D_{j}\bar{G}_{j}S_{j}\chi=0,~{}\chi^{\top}S_{j}^{\top}G_{j}^{\top}M_{j}\bar{G}_{j}S_{j}\chi\geq
0$ $\displaystyle\forall j=1,\dots,\xi.\\}$ (6)

where $G_{j}=[0\ \mathbf{0}\ -I_{n}\ I_{n}]$, $\bar{G}_{j}=[-d_{j}\ -E_{j}\
\mathbf{0}\ I_{n}]$ for $j=2,\dots\xi$ and $G_{1}=[1\ \mathbf{0}\
\mathbf{0}]$, $\bar{G}_{1}=[-d_{1}\ -E_{1}\ I_{n}]$. Then

$\chi(x)\in\mathfrak{X}(x).$

###### Proof:

First see that for any $j=2,\dots,\xi$, we have
$G_{j}S_{j}\chi(x)=\lambda_{j}(x)-\lambda_{j-1}(x)$ and
$\bar{G}_{j}S_{j}\chi(x)=\lambda_{j}(x)-A_{j}x-d_{j}$. For the equality in
(IV), we have

$\displaystyle(\lambda_{j}(x)-\lambda_{j-1}(x))^{\top}(\lambda_{j}(x)-E_{j}x-d_{j})=0$
$\displaystyle\Rightarrow$
$\displaystyle([\lambda_{j}(x)-\lambda_{j-1}(x)]_{i})[D_{j}]_{ii}([\lambda_{j}(x)-E_{j}x-d_{j}]_{i})=0,$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\forall
i=1,\dots,n$ $\displaystyle\Leftrightarrow$
$\displaystyle\chi^{\top}S_{j}^{\top}G^{\top}D_{j}\bar{G}_{j}S_{j}\chi=0$

where $D_{j}\in\mathbb{R}^{n\times n}$ is any diagonal matrix. For the
inequalities in (IV), we have for any non-negative matrix
$M_{j}\in\mathbb{R^{+}}^{n\times n}$ that

$\displaystyle\lambda_{j}(x)\geq\lambda_{j-1}(x),\ \lambda_{j}(x)\geq
E_{j}x+d_{j}$ $\displaystyle\Rightarrow($
$\displaystyle[\lambda_{j}(x)-\lambda_{j-1}(x)]_{i})([\lambda_{j}(x)-E_{j}x-d_{j}]_{k})\geq
0,$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\forall
i,k=1,\dots,n$ $\displaystyle\Rightarrow($
$\displaystyle[\lambda_{j}(x)-\lambda_{j-1}(x)]_{i})[M_{j}]_{ik}([\lambda_{j}(x)-E_{j}x-d_{j}]_{k})\geq
0,$
$\displaystyle~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\forall
i,k=1,\dots,n$ $\displaystyle\Leftrightarrow$
$\displaystyle\chi^{\top}S_{j}^{\top}G^{\top}M_{j}G_{j}S_{j}\chi\geq 0.$

For $j=1$, see that $G_{1}S_{1}\chi(x)=1$ and
$\bar{G}_{1}S_{1}\chi(x)=\lambda_{1}(x)-E_{1}x-d_{1}$. Proceeding as above, we
get the result for $j=1$ where the inequality
$\chi(x)S_{1}^{\top}G_{1}^{\top}M_{1}\bar{G}_{1}S_{1}\chi(x)\geq 0$ holds
trivially with equality.
$\blacksquare$ ∎

The above proposition provides us with a tool to construct a sufficiency
certificate for a quadratic inequality $[1\ x^{\top}\ \lambda_{1}^{\top}\
\dots\lambda_{\xi}^{\top}]P[1\ x^{\top}\ \lambda_{1}^{\top}\
\dots\lambda_{\xi}^{\top}]^{\top}\geq 0$ to hold given that $[1\
x^{\top}\lambda_{1}^{\top}\dots\lambda_{\xi}^{\top}]^{\top}=\chi(x)$. Using
the S-procedure, if $\exists D_{j}\in\mathbb{R}^{n\times
n},M_{j}\in\mathbb{R^{+}}^{n\times n}$ for $j=1,\dots,\xi$ such that

$P-\sum_{j=1}^{\xi}\text{He}(S^{\top}_{j}G_{j}^{\top}(M_{j}+D_{j})\bar{G}_{j}S_{j})\geq
0,$

then $\chi(x)^{\top}P\chi(x)\geq 0$. This can be readily verified by
multiplying both sides of the above inequality by $\chi(x)$ and using
Proposition 1.

To use Proposition 1 for a general continuous PWA function
$\lambda:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$, we first obtain its DC
decomposition $\lambda(x)=\gamma(x)-\eta(x)$. Then we can use proposition 1
for $\gamma(\cdot)$ and $\eta(\cdot)$ to characterize the set
$(x,\gamma(x),\eta(x))$ in the space
$(x,\gamma_{1},\dots,\gamma_{\alpha},\eta_{1},\dots,\eta_{\beta})\in\mathbb{R}^{n(1+\alpha+\beta)}$.
We illustrate this procedure by synthesizing a PWA policy and PWQ Lyapunov
function for the uncertain DC system (2).

##  V Synthesis of Stabilizing PWA Policies

In this section, we provide a procedure for synthesizing stabilizing PWA
feedback policies for uncertain DC systems (2) using PWQ Control Lyapunov
Functions (CLFs) as stability certificates. First, we provide conditions under
which a function qualifies as a CLF which in turn, establishes the existence
of a stabilizing controller for system (2). Second, we propose a redefinition
of the DC decomposition in model (2) without changing the system dynamics.
Finally, we use the redefined model to synthesize a PWQ CLF and a PWA control
policy using Linear Matrix Inequalties (LMIs).

###  V-A Stability via Control Lyapunov Functions

###### Lemma 1

Suppose that there exists a function
$V:\mathbb{R}^{+}\times\mathcal{X}\rightarrow\mathbb{R}$ and scalars
$\rho_{1},\rho_{2}\in\mathbb{R}^{+},\rho_{3}\in(0,1)$ such that:

  1. 1.

$\rho_{1}\|x\|^{2}\leq V(t,x)\leq\rho_{2}\|x\|^{2}\ \forall
x\in\mathcal{X},t\geq 0$

  2. 2.

$\forall t\geq 0,x(t)\in\mathcal{X},\exists u^{*}(t,x(t))\in\mathcal{U}:$
$V(t+1,x(t+1))\leq\rho_{3}V(t,x(t)),\ \forall\mathbf{W}(t)\in\mathcal{W}$.

Then $V(t,x)$ is a Control Lyapunov function for system (2), and the origin of
the closed-loop system with $u(t)=u^{*}(t,x(t))$ is exponentially stable.

###### Proof:

Let $V(t,x)$ be a CLF for system (2). Then for any initial condition
$x(0)=x_{0}\in\mathcal{X}$, we have $V(0,x(0))\leq\rho_{2}\|x_{0}\|^{2}$ using
the second inequality in property 1). Using property 2) for trajectories of
the uncertain system (2) in closed-loop with $u^{*}(t,x(t))$, we also have

$V(t,x(t))\leq\rho_{3}V(t-1,x(t-1))\leq\dots\leq\rho_{3}^{t}V(0,x(0))$

for all $\mathbf{W}(0),\dots,\mathbf{W}(t-1)\in\mathcal{W}$. Using the first
inequality in property 1), we get $\rho_{1}\|x(t)\|^{2}\leq V(t,x(t))$.
Combining these inequalities, we get
$\|x(t)\|\leq\sqrt{\rho_{3}^{t}\frac{\rho_{2}}{\rho_{1}}}\|x_{0}\|$ which in
turn, implies $\lim_{t\rightarrow\infty}\|x(t)\|=0$ because $\rho_{3}\in(0,1)$
and $\frac{\rho_{2}}{\rho_{1}}>0$. Since $x_{0}$ was arbitrary, the proof is
complete.
$\blacksquare$ ∎

In order to present CLF candidates for system (2), we redefine its DC
decomposition without effectively changing the dynamics in the next section.

###  V-B Redefinition of DC Decomposition

###### Lemma 2

Let $d_{*}^{k}\in\mathbb{R}^{n}$ such that

$[d_{*}^{k}]_{i}=\max_{1\leq j\leq\alpha}[d_{j}^{k}]_{i}=\max_{1\leq
l\leq\beta}[f_{j}^{k}]_{i}$

for system (2), which exists by assumption 3. Let
$\tilde{E}^{k}_{j}=E^{k}_{j},\tilde{d}^{k}_{j}=d^{k}_{j}-d^{k}_{*}$, for
$j=1,\dots,\alpha$ and
$\tilde{H}^{k}_{j}=H^{k}_{j},\tilde{f}^{k}_{l}=f^{k}_{l}-d^{k}_{*}$ , for
$l=1,\dots,\beta$. Accordingly redefine the matrices $\tilde{\mathbf{W}}(t)$,
$\tilde{W}^{k}$ and polytope $\tilde{\mathcal{W}}$ as in Assumption 2. Then
the redefined DC decomposition of system (2) given by

$\displaystyle x(t+1)$
$\displaystyle=Ax(t)+\tilde{\gamma}(t,x(t))-\tilde{\eta}(t,x(t))+Bu(t),$ (7)
$\displaystyle[\tilde{\gamma}(t,x)]_{i}$ $\displaystyle=\max_{1\leq
j\leq\alpha}\ [\tilde{E}_{j}(t)]_{i}x+[\tilde{d}_{j}(t)]_{i},$
$\displaystyle[\tilde{\eta}(t,x)]_{i}$ $\displaystyle=\max_{1\leq l\leq\beta}\
[\tilde{H}_{l}(t)]_{i}x+[\tilde{f}_{l}(t)]_{i},~{}~{}i=1,\dots,n$

satisfies the following properties

  1. 1.

$\gamma(t,x)-\eta(t,x)=\tilde{\gamma}(t,x)-\tilde{\eta}(t,x),\forall t\geq
0,x\in\mathcal{X}$

  2. 2.

$\tilde{\gamma}(t,\mathbf{0})=\tilde{\eta}(t,\mathbf{0})=\mathbf{0},\forall
t\geq 0$

  3. 3.

$\exists C_{\gamma},C_{\eta}\in\mathbb{R}^{+}\ :\|\tilde{\gamma}(t,x)\|\leq
C_{\gamma}\|x\|,\|\tilde{\eta}(t,x)\|\leq C_{\eta}\|x\|$.

###### Proof:

  1. 1.

We have $\mathbf{W}(t)\in\mathcal{W}$ from Assumption 2, therefore

$\displaystyle\forall t\geq 0,\ \exists
v(t)\in(\mathbb{R}^{+})^{n_{V}},\mathbf{1}^{\top}v(t)=1:$
$\displaystyle\sum_{k=1}^{n_{V}}[v(t)]_{k}W^{k}=\mathbf{W}(t).$

Define $d_{*}(t)=\sum_{k=1}^{n_{V}}[v(t)]_{k}d^{k}_{*}$ and see that for any
$t\geq 0,x\in\mathcal{X}$,

$\displaystyle[\gamma(t,x)-\eta(t,x)]_{i}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}$
$\displaystyle~{}~{}=\max_{1\leq j\leq\alpha}\
[E_{j}(t)]_{i}x(t)+[d_{j}(t)]_{i}$ $\displaystyle~{}~{}~{}~{}-\max_{1\leq
l\leq\beta}\ [H_{l}(t)]_{i}x(t)+[f_{l}(t)]_{i}$
$\displaystyle~{}~{}=-d_{*}(t)+\max_{1\leq j\leq\alpha}\
[E_{j}(t)]_{i}x(t)+[d_{j}(t)]_{i}$
$\displaystyle~{}~{}~{}~{}+d_{*}(t)-\max_{1\leq l\leq\beta}\
[H_{l}(t)]_{i}x(t)+[f_{l}(t)]_{i}$
$\displaystyle~{}~{}=\sum_{k=1}^{n_{V}}[v(t)]_{k}\big{(}\max_{1\leq
j\leq\alpha}\ [E_{j}^{k}]_{i}x(t)+[d_{j}^{k}-d^{k}_{*}]_{i}$
$\displaystyle~{}~{}~{}~{}-\max_{1\leq l\leq\beta}\
[H_{l}^{k}]_{i}x(t)+[f_{l}^{k}-d^{k}_{*}]_{i}\big{)}$
$\displaystyle~{}~{}=\sum_{k=1}^{n_{V}}[v(t)]_{k}\big{(}\max_{1\leq
j\leq\alpha}\ [\tilde{E}_{j}^{k}]_{i}x(t)+[\tilde{d}_{j}^{k}]_{i}$
$\displaystyle~{}~{}~{}~{}-\max_{1\leq l\leq\beta}\
[\tilde{H}_{l}^{k}]_{i}x(t)+[\tilde{f}_{l}^{k}]_{i}\big{)}$
$\displaystyle~{}~{}=[\tilde{\gamma}(t,x)-\tilde{\eta}(t,x)]_{i},~{}~{}i=1,\dots,n.$

  2. 2.

Let $\tilde{j}^{i}_{k}=\text{arg}\max_{1\leq j\leq\alpha}[d_{j}^{k}]_{i}$ and
$\tilde{l}^{i}_{k}=\text{arg}\max_{1\leq l\leq\beta}[f_{l}^{k}]_{i}$. By
Assumption 3, we have
$[d^{k}_{\tilde{j}^{i}_{k}}]_{i}=[f^{k}_{\tilde{l}^{i}_{k}}]_{i}=[d^{k}_{*}]_{i}$.
Therefore, $\tilde{d}^{k}_{j},\tilde{f}^{k}_{j}\leq 0$ and $\forall
i=1,\dots,n$,

$\displaystyle[\tilde{\gamma}(t,\mathbf{0})]_{i}=\max_{1\leq
j\leq\alpha}[\tilde{d}^{j}_{k}]_{i}=[d^{k}_{\tilde{j}^{i}_{k}}]_{i}-[d^{k}_{*}]_{i}=0,$
$\displaystyle[\tilde{\eta}(t,\mathbf{0})]_{i}=\max_{1\leq
l\leq\beta}[\tilde{f}^{j}_{k}]_{i}=[f^{k}_{\tilde{l}^{i}_{k}}]_{i}-[d^{k}_{*}]_{i}=0.$

  3. 3.

Let $C_{\gamma}=\max_{\begin{subarray}{c}1\leq j\leq\alpha\\\ 1\leq k\leq
n_{V}\end{subarray}}\|E^{k}_{j}\|_{F}$ (where $\|\cdot\|_{F}$ is the Frobenius
norm). We have already seen in 2) that $\tilde{d}^{k}_{j}\leq 0$ and so we
have

$\displaystyle E^{k}_{j}x+\tilde{d}^{k}_{j}\leq E^{k}_{j}x,\forall
j=1,\dots,\alpha$ $\displaystyle\Rightarrow$
$\displaystyle\sum_{k=1}^{n_{V}}[v(t)]_{k}(\max_{1\leq
j\leq\alpha}E^{k}_{j}x+\tilde{d}^{k}_{j})\leq\sum_{k=1}^{n_{V}}[v(t)]_{k}(\max_{1\leq
j\leq\alpha}E^{k}_{j}x)$ $\displaystyle\Rightarrow$
$\displaystyle\tilde{\gamma}(t,x)\leq\sum_{k=1}^{n_{V}}[v(t)]_{k}(\max_{1\leq
j\leq\alpha}E^{k}_{j}x)$ $\displaystyle\Rightarrow$
$\displaystyle\|\tilde{\gamma}(t,x)\|\leq(\sum_{k=1}^{n_{V}}[v(t)]_{k})C_{\gamma}\|x\|=C_{\gamma}\|x\|$

By setting $C_{\eta}=\max_{\begin{subarray}{c}1\leq l\leq\beta\\\ 1\leq k\leq
n_{V}\end{subarray}}\|H^{k}_{l}\|_{F}$, we can similarly show
$\|\tilde{\eta}(t,x)\|\leq C_{\eta}\|x\|$.

$\blacksquare$ ∎

###### Remark 3

If $\gamma(t,\mathbf{0})=\eta(t,\mathbf{0})=\Gamma~{}\forall t\geq 0$ for some
constant, known $\Gamma\in\mathbb{R}^{n}$ then
$\tilde{\gamma}(t,x)=\gamma(t,x)-\Gamma$ and
$\tilde{\eta}(t,x)=\eta(t,x)-\Gamma$.

###  V-C Control Policy Synthesis

We consolidate our results to construct a PWQ CLF $V(t,x(t))$ and stabilizing
PWA policy $u^{*}(t,x(t))$ satisfying the properties in Lemma 1 for system (2)
using the technique presented in section IV and the redefined dynamics (7)
from Lemma 2. The main idea is to express the convex PWA functions
$\tilde{\gamma}(\cdot)$, $\tilde{\eta}(\cdot)$ as the solution to a set of
linear inequalities and quadratic equalities in a lifted space (as in (IV))
and then derive certificates to check if the candidate $V(\cdot)$ and
$u^{*}(\cdot)$ satisfies the properties given in Lemma 1.

Consider CLF candidates for system (7) (or equivalently, (2)) of the form

$\displaystyle V(t,x)=\begin{bmatrix}x\\\ \tilde{\gamma}(t,x)\\\
\tilde{\eta}(t,x)\end{bmatrix}^{\top}P\begin{bmatrix}x\\\
\tilde{\gamma}(t,x)\\\ \tilde{\eta}(t,x)\end{bmatrix}.$ (8)

Note that since $\tilde{\gamma}(t,\cdot),\tilde{\eta}(t,\cdot)$ are PWA in
$x$, $V(t,\cdot)$ is PWQ in $x$. The polytopic partition and the quadratic
functions describing $V(t,\cdot)$ are implicitly defined by
$\tilde{\gamma}(t,\cdot)$ and $\tilde{\eta}(t,\cdot)$.

Now we proceed to construct the policy class for our stabilizing policy
$u^{*}(t,x(t)$. Recursively define the following functions

$\displaystyle\tilde{\gamma}_{j}(t,x)=\max\\{\tilde{\gamma}_{j-1}(t,x),\tilde{E}_{j}(t)x+\tilde{d}_{j}(t)\\}~{}~{}j=2,\dots,\alpha$
$\displaystyle\tilde{\eta}_{l}(t,x)=\max\\{\tilde{\eta}_{l-1}(t,x),\tilde{H}_{l}(t)x+\tilde{f}_{l}(t)\\}~{}~{}l=2,\dots,\beta$

with $\tilde{\gamma}_{1}(t,x)=\tilde{E}_{1}(t)x+\tilde{d}_{1}(t)$ and
$\tilde{\eta}_{1}(t,x)=\tilde{H}_{1}(t)x+\tilde{f}_{1}(t)$ and define

$\displaystyle\chi(t,x)=[1\ x^{\top}\ \tilde{\gamma}_{1}(t,x)^{\top}\
\dots\tilde{\gamma}_{\alpha}(t,x)^{\top}\ \tilde{\eta}_{1}(t,x)^{\top}$
$\displaystyle\dots\tilde{\eta}_{\beta}(t,x)^{\top}]^{\top}.$ (9)

Due to the nested maximizations property as noted earlier, we have
$\tilde{\gamma}(t,x)=\tilde{\gamma}_{\alpha}(t,x)$ and
$\tilde{\eta}(t,x)=\tilde{\eta}_{\beta}(t,x)$. Similar to (IV), we have for
any $t\geq 0$ that $(x,\tilde{\gamma}(t,x),\tilde{\eta}(t,x))$ is given as the
solution to the following linear inequalities and quadratic equalities in the
space
$(x,\tilde{\gamma}_{1},\dots,\tilde{\gamma}_{\alpha},\tilde{\eta}_{1},\dots,\tilde{\eta}_{\beta})\in\mathbb{R}^{n(1+\alpha+\beta)}$
(where $x,\tilde{\gamma}_{1},\dots,\tilde{\eta}_{\beta}$ are arbitrary).

$\displaystyle\tilde{\gamma}_{1}=\tilde{E}_{1}(t)x+\tilde{d}_{1}(t),$ (10)
$\displaystyle\tilde{\gamma}_{j}\geq\tilde{\gamma}_{j-1},\
\tilde{\gamma}_{j}\geq\tilde{E}_{j}(t)x+\tilde{d}_{j}(t),$
$\displaystyle(\tilde{\gamma}_{j}-\tilde{\gamma}_{j-1})^{\top}(\tilde{\gamma}_{j}-\tilde{E}_{j}(t)x-\tilde{d}_{j}(t))=0,\
j=2,\dots,\alpha,$
$\displaystyle\tilde{\eta}_{1}=\tilde{H}_{1}(t)x+\tilde{f}_{1}(t),$
$\displaystyle\tilde{\eta}_{l}\geq\tilde{\eta}_{l-1},\
\tilde{\eta}_{l}\geq\tilde{E}_{l}(t)x+\tilde{f}_{l}(t),$
$\displaystyle(\tilde{\eta}_{l}-\tilde{\eta}_{l-1})^{\top}(\tilde{\eta}_{l}-\tilde{E}_{l}(t)x-\tilde{f}_{l}(t))=0,\
l=2,\dots,\beta.$

For defining our policy class, we make the following assumption on the
feedback structure of system (2).

###### Assumption 4

There exists a known matrix $C\in\mathbb{R}^{p\times(1+n(1+\alpha+\beta))}$
such that $C\chi(t,x)$ is observed.

With this assumption, we consider policy candidates parametrized by
$K\in\mathbb{R}^{m\times p}$ of the form

$\displaystyle u(t,x)=KC\chi(t,x).$ (11)

Since $\chi(t,\cdot)$ as defined in (V-C) is PWA in $x$, the policy
$u(t,\cdot)$ is PWA in $x$ where the time-varying polytopic partition
$\sqcup_{i=1}^{n_{r}(t)}\tilde{\mathcal{X}}_{i}(t)=\mathcal{X}$ and affine
functions $F_{i}(t)x+g_{i}$ in (3) are defined implicitly.

The following theorem is our main result that describes LMIs whose solution
yields a CLF $V(t,x(t))$ and stabilizing policy $u(t,x(t))$ of the form
discussed above. Before stating the theorem, we require some definitions. Let

$\displaystyle
x^{+}=Ax+\tilde{\gamma}_{\alpha}(t,x)-\tilde{\eta}_{\beta}(t,x)+BKC\chi(t,x)$
(12)
$\displaystyle\chi^{+}(t,x)=[\chi(t,x)^{\top}\tilde{\gamma}_{1}(t+1,x^{+})^{\top}\dots\tilde{\gamma}_{\alpha}(t+1,x^{+})^{\top}$
$\displaystyle\tilde{\eta}_{1}(t+1,x^{+})^{\top}\
\dots\tilde{\eta}_{\beta}(t+1,x^{+})^{\top}]^{\top}$

and define the following selection matrices:

  * •

$S$ such that $S\chi^{+}(t,x)=\chi(t,x)$,

  * •

$S_{\lambda}$ such that $S_{\lambda}\chi^{+}(t,x)=[1\ x^{\top}\
\tilde{\gamma}(t,x)^{\top}\ \tilde{\eta}(t,x)^{\top}]^{\top}$,

  * •

$S_{x}$ such that $S_{x}\chi^{+}(t,x)=[1\ x^{\top}]^{\top}$,

  * •

$S^{+}$ such that $S^{+}\chi^{+}(t,x)=\chi(t+1,x^{+})$,

  * •

$G_{\gamma 1},G_{\eta 1}$ such that $G_{\gamma 1}\chi(t,x)=I_{n}$, $G_{\eta
1}\chi(t,x)=I_{n}$ respectively,

  * •

$G_{\gamma j},G_{\eta l}$ such that $G_{\gamma
j}\chi(t,x)=\tilde{\gamma}_{j}(t,x)-\tilde{\gamma}_{j-1}(t,x)$, $G_{\eta
l}\chi(t,x)=\tilde{\eta}_{l}(t,x)-\tilde{\eta}_{l-1}(t,x)$ for
$j=2,\dots,\alpha$ and $l=2,\dots,\beta$ respectively,

  * •

$G_{\gamma j}^{k}$, $G_{\eta l}^{k}$ such that $G^{k}_{\gamma
j}\chi(t,x)=\tilde{\gamma}_{j}(t,x)-\tilde{E}_{j}^{k}x-\tilde{d}_{j}^{k}$,
$G^{k}_{\eta
l}\chi(t,x)=\tilde{\eta}_{l}(t,x)-\tilde{H}_{l}^{k}x-\tilde{f}_{l}^{k}$ for
$j=1,\dots,\alpha$ and $l=1,\dots,\beta$ respectively.

Note that $S^{+}$ is affine in the unknown gain $K$ from (12). All the other
matrices are constant, known matrices that depend on the problem data.

###### Theorem 1

Fix some $\rho_{1}\in\mathbb{R}^{+},\rho_{3}\in(0,1)$. Define the ellipsoidal
set $\tilde{\mathcal{U}}=\\{u\in\mathbb{R}^{m}:\ u^{\top}Q_{u}u\leq
1\\}\subset\mathcal{U}$ where $Q_{u}^{\top}=Q_{u}$, $Q_{u}\succ 0$. Suppose
that there exist real-valued matrices $T,V$, diagonal matrix $J$, non-negative
matrices $M_{\gamma j}$, $M_{\eta l}$, $\bar{M}_{\gamma j}$, $\bar{M}_{\eta
l}$, $\tilde{M}_{\gamma j}$, $\tilde{M}_{\eta l}$, $M^{+}_{\gamma j}$,
$M^{+}_{\eta l}$, diagonal matrices $D_{\gamma l}$, $D_{\eta l}$,
$\bar{D}_{\gamma j}$, $\bar{D}_{\eta l}$ $\tilde{D}_{\gamma l}$,
$\tilde{D}_{\eta l}$, $D^{+}_{\gamma l}$, $D^{+}_{\eta l}$ for
$j=1,\dots,\alpha$ and $l=1,\dots,\beta$, such that for some symmetric matrix
$P^{*}$ and feedback gain $K^{*}$ we have:

(Positive-definite Lyapunov Function)
$\displaystyle(S_{\lambda}S)^{\top}P^{*}S_{\lambda}S-S_{x}^{\top}\begin{bmatrix}0&\mathbf{0}\\\
\mathbf{0}&\rho_{1}I_{n}\end{bmatrix}S_{x}$
$\displaystyle-S^{\top}\text{He}(\sum_{j=1}^{\alpha}G^{\top}_{\gamma
j}(D_{\gamma j}+M_{\gamma j})G_{\gamma j}^{k}+\sum_{l=1}^{\beta}G^{\top}_{\eta
l}(D_{\eta l}+M_{\eta l})G_{\eta l}^{k})S\succeq 0$
$\displaystyle~{}~{}~{}~{}~{}k=1,\dots,n_{V}$ (13)
$\displaystyle~{}~{}~{}~{}~{}~{}~{}[P^{*}]_{11}=0$ (14)

(Input Constraints) $\displaystyle(CS)^{\top}\begin{bmatrix}1&\mathbf{0}\\\
\mathbf{0}&-Q_{u}\end{bmatrix}CS$
$\displaystyle-S^{\top}\text{He}(\sum_{j=1}^{\alpha}G^{\top}_{\gamma
j}(\bar{D}_{\gamma j}+\bar{M}_{\gamma j})G_{\gamma
j}^{k}+\sum_{l=1}^{\beta}G^{\top}_{\eta l}(\bar{D}_{\eta l}+\bar{M}_{\eta
l})G_{\eta l}^{k})S\succeq 0$ $\displaystyle~{}~{}~{}~{}~{}k=1,\dots,n_{V}$
(15)

(Decreasing Lyapunov Function)
$\displaystyle\left[\begin{array}[]{ccc}Z^{k}_{1}&V&\hbox{\multirowsetup$\mathbf{Z}^{q}_{2}$}\\\
\star&2\text{He}(T)-Y&\\\
\lx@intercol\hfil\star\hfil\lx@intercol&Z_{3}\end{array}\right]\succeq 0$ (19)
$\displaystyle~{}~{}~{}~{}k=1,\dots,n_{V},~{}q=1,\dots,n_{V}$ (20)

where

$\displaystyle Z^{k}_{1}$ $\displaystyle=(1-\rho_{3})S^{\top}P^{*}S$
$\displaystyle-S^{\top}\text{He}(\sum_{j=1}^{\alpha}G^{\top}_{\gamma
j}(\tilde{D}_{\gamma j}+\tilde{M}_{\gamma j})G_{\gamma
j}^{k}+\sum_{l=1}^{\beta}G^{\top}_{\eta l}(\tilde{D}_{\eta l}+\tilde{M}_{\eta
l})G_{\eta l}^{k})S,$ $\displaystyle{Z}^{q}_{2}$
$\displaystyle=\begin{bmatrix}U^{q}&V\\\
\mathbf{0}&T^{\top}\end{bmatrix},\quad Z_{3}=\begin{bmatrix}J&\mathbf{0}\\\
\star&2I-J\end{bmatrix},$ $\displaystyle U^{q}$
$\displaystyle=S^{+\top}[S^{\top}_{\lambda}\ G^{\top}_{\gamma 1}\ G_{\gamma
1}^{q\top}\dots\ G^{\top}_{\gamma\alpha}\ G_{\gamma\alpha}^{q\top}\
G^{\top}_{\eta 1}\ G_{\eta 1}^{q\top}\dots G^{\top}_{\eta\beta}\
G_{\eta\beta}^{q\top}],$ $\displaystyle Y$
$\displaystyle=\text{blkdiag}(P^{*},Y_{\gamma
1},\dots,Y_{\gamma\alpha},Y_{\eta 1},\dots,Y_{\eta\beta}),$ $\displaystyle
Y_{\gamma j}$ $\displaystyle=\begin{bmatrix}\mathbf{0}&D^{+}_{\gamma
j}+M^{+}_{\gamma j}\\\ \star&\mathbf{0}\end{bmatrix},Y_{\eta
j}=\begin{bmatrix}\mathbf{0}&D^{+}_{\eta j}+M^{+}_{\eta j}\\\
\star&\mathbf{0}\end{bmatrix}.$

Then the origin of the closed-loop system (2) with
$u^{*}(t,x)=K^{*}C\chi(t,x)$ is exponentially stable and
$u^{*}(t,x)\in\mathcal{U}~{}\forall x\in\mathcal{X},t\geq 0$.

###### Proof:

Multiplying the $k$th inequality in (1) by $\chi^{+}(t,x)$ on both sides for
$x\in\mathcal{X},t\geq 0$, gives

$\displaystyle V(t,x)-\rho_{1}\|x\|^{2}$
$\displaystyle\geq\sum_{j=1}^{\alpha}\chi^{\top}(t,x)G^{\top}_{\gamma
j}(D_{\gamma j}+M_{\gamma j})G_{\gamma j}^{k}\chi(t,x)$
$\displaystyle+\sum_{l=1}^{\beta}\chi^{\top}(t,x)G^{\top}_{\eta l}(D_{\eta
l}+M_{\eta l})G_{\eta l}^{k}\chi(t,x).$

From Assumption 2, we know that $\mathbf{W}(t)\in\mathcal{W}\Rightarrow\forall
t\geq 0,\exists
v(t)\in(\mathbb{R}^{+})^{n_{V}},\mathbf{1}^{\top}v(t)=1:\sum_{k=1}^{n_{V}}[v(t)]_{k}W^{k}=\mathbf{W}(t)$.
Also notice that the matrices $G^{k}_{\gamma j},G^{k}_{\eta j}$ are linear in
$W^{k}$. Let $\bar{G}_{\gamma j}(t)$, $\bar{G}_{\eta l}(t)$ be matrices such
that
$\tilde{\gamma}_{j}(t,x)-\tilde{E}_{j}(t)x-\tilde{d}_{j}(t)=\bar{G}_{\gamma
j}(t)\chi(t,x)$,
$\tilde{\eta}_{l}(t,x)-\tilde{H}_{l}(t)x-\tilde{f}_{l}(t)=\bar{G}_{\eta
l}(t)\chi(t,x)$ for $j=1,\dots,\alpha$ and $l=1,\dots,\beta$ respectively.
Then we have $\bar{G}_{\gamma j}=\sum_{k=1}^{n_{V}}[v(t)]_{k}G^{k}_{\gamma j}$
and $\bar{G}_{\eta l}=\sum_{k=1}^{n_{V}}[v(t)]_{k}G^{k}_{\eta l}$. Thus
multiplying the $k$th inequality above by $[v(t)]_{k}$ and adding all gives

$\displaystyle V(t,x)-\rho_{1}\|x\|^{2}\geq$
$\displaystyle\sum_{j=1}^{\alpha}\chi(t,x)^{\top}G^{\top}_{\gamma j}(D_{\gamma
j}+M_{\gamma j})\bar{G}_{\gamma j}(t)\chi(t,x)$
$\displaystyle+\sum_{l=1}^{\beta}\chi(t,x)^{\top}G^{\top}_{\eta l}(D_{\eta
l}+M_{\eta l})\bar{G}_{\eta l}(t)\chi(t,x)$

From Proposition 1, we have that the two terms on the right are positive and
so

$\rho_{1}\|x\|^{2}\leq V(t,x)~{}~{}\forall x\in\mathcal{X},t\geq 0.$

Similarly, (1) gives for any $x\in\mathcal{X},t\geq 0$

$\displaystyle 1-u^{*}(t,x)^{\top}Q_{u}u^{*}(t,x)\geq$
$\displaystyle\sum_{j=1}^{\alpha}\chi(t,x)^{\top}G^{\top}_{\gamma j}(D_{\gamma
j}+M_{\gamma j})\bar{G}_{\gamma j}(t)\chi(t,x)$
$\displaystyle+\sum_{l=1}^{\beta}\chi(t,x)^{\top}G^{\top}_{\eta l}(D_{\eta
l}+M_{\eta l})\bar{G}_{\eta l}(t)\chi(t,x)$

for any $x\in\mathcal{X},t\geq 0$. From Proposition 1, we have that the two
terms on the right are positive and so

$u^{*}(t,x)\in\tilde{\mathcal{U}}\subset\mathcal{U}~{}\forall
x\in\mathcal{X},t\geq 0.$

From lemma 2, we have $\|\tilde{\gamma}(t,x)\|\leq C_{\gamma}\|x\|$,
$\|\tilde{\eta}(t,x)\|\leq C_{\eta}\|x\|$ for some
$C_{\gamma},C_{\eta}\in\mathbb{R}^{+}$. Using (14) and setting
$\rho_{2}=\lambda_{max}(P^{*})(1+C_{\gamma}^{2}+C_{\eta}^{2})$, we get

$V(t,x)\leq\rho_{2}\|x\|^{2}~{}~{}\forall x\in\mathcal{X},t\geq 0.$

Observe that $Z^{k}_{1},Z^{q}_{2}$ in inequality (1) are linear in
$G^{k}_{\gamma j},G^{k}_{\eta l}$ and $G^{q}_{\gamma j},G^{q}_{\eta l}$
respectively. Define $Z_{1}(t)$ by replacing $G^{k}_{\gamma j},G^{k}_{\eta l}$
with $\bar{G}_{\gamma j}(t)$, $\bar{G}_{\eta l}(t)$ in the definition of
$Z^{k}_{1}$ and see that $Z_{1}(t)=\sum_{k=1}^{n_{V}}[v(t)]_{k}Z^{k}_{1}$.
Similarly, define $Z_{2}(t+1)$ by replacing $G^{q}_{\gamma j},G^{q}_{\eta l}$
with $\bar{G}_{\gamma j}(t+1)$, $\bar{G}_{\eta l}(t+1)$ in the definition of
$Z^{q}_{2}$ and see that $Z_{2}(t+1)=\sum_{q=1}^{n_{V}}[v(t+1)]_{q}Z^{q}_{2}$.
For a fixed $q$, multiplying the $k$th inequality in (1) by $[v(t)]_{k}$ and
adding the $n_{V}$ inequalities gives

$\left[\begin{array}[]{ccc}Z_{1}(t)&V&\hbox{\multirowsetup$\mathbf{Z}^{q}_{2}$}\\\
\star&2\text{He}(T)-Y&\\\
\lx@intercol\hfil\star\hfil\lx@intercol&Z_{3}\end{array}\right]\succeq 0.$

Again, multiplying the $q$th inequality above with $[v(t+1)]_{q}$ and adding
the $n_{V}$ resulting inequalities gives

$\left[\begin{array}[]{ccc}Z_{1}(t)&V&\hbox{\multirowsetup$Z_{2}(t+1)$}\\\
\star&2\text{He}(T)-Y&\\\
\lx@intercol\hfil\star\hfil\lx@intercol&Z_{3}\end{array}\right]\succeq 0.$

Now we proceed to eliminate the variables $J,T$ and $V$ as follows. Using the
AM-GM inequality, we have $J^{-1}\succeq 2I-J$ which we replace in $Z_{3}$.
Then, we use Schur’s complement and Young’s inequality to eliminate $J$.
Variables $T$ and $V$ are eliminated by employing the reciprocal-projection
lemma ([18, Section 2.5.4, 1]). Multiplying both sides of the resulting
inequality with $\chi^{+}(t,x)$ for $x\in\mathcal{X}$, gives

$\displaystyle-(V(t+1,x^{+})-\rho_{3}V(t,x))\geq$
$\displaystyle\sum_{j=1}^{\alpha}\chi(t,x)^{\top}G^{\top}_{\gamma
j}(\tilde{D}_{\gamma j}+\tilde{M}_{\gamma j})\bar{G}_{\gamma j}(t)\chi(t,x)+$
$\displaystyle\sum_{l=1}^{\beta}\chi(t,x)^{\top}G^{\top}_{\eta
l}(\tilde{D}_{\eta l}+\tilde{M}_{\eta l})\bar{G}_{\eta l}(t)\chi(t,x)+$
$\displaystyle\sum_{j=1}^{\alpha}\chi(t+1,x^{+})^{\top}G^{\top}_{\gamma
j}(D^{+}_{\gamma j}+M^{+}_{\gamma j})\bar{G}_{\gamma j}(t+1)\chi(t+1,x^{+})+$
$\displaystyle\sum_{l=1}^{\beta}\chi(t+1,x^{+})^{\top}G^{\top}_{\eta
l}(D^{+}_{\eta l}+M^{+}_{\eta l})\bar{G}_{\eta l}(t+1)\chi(t+1,x^{+}).$

From Proposition 1, we have that the four terms on the right are positive and
so

$V(t+1,x^{+})\leq\rho_{3}V(t,x)~{}~{}\forall x\in\mathcal{X},t\geq 0.$

Thus by Lemma 1, the origin of the closed-loop system (2) with
$u^{*}(t,x)=K^{*}C\chi(t,x)$ is exponentially stable.
$\blacksquare$ ∎

###### Remark 4

Note that although (1),(14) make $V(t,x)$ positive-definite on
$\mathbb{R}^{n}$, the matrix $P^{*}\in\mathbb{R}^{(1+3n)\times(1+3n)}$ can be
indefinite.

###### Remark 5

The LMI (1) can be conservative in practice. Alternatively, it can be replaced
by the following BMI.

(Decreasing Lyapunov Function)
$\displaystyle\begin{bmatrix}\mathbf{Z}^{k}_{1}&\mathbf{Z}^{q}_{2}\\\
\star&\mathbf{Z}_{3}\end{bmatrix}\succeq 0$
$\displaystyle~{}~{}~{}~{}k=1,\dots,n_{V},~{}q=1,\dots,n_{V}$ (21)

where

$\displaystyle\mathbf{Z}^{k}_{1}$
$\displaystyle=(1-\rho_{3})S^{\top}P^{*}S+\tilde{r}S_{x}^{\top}\begin{bmatrix}-1&\mathbf{0}\\\
\mathbf{0}&Q_{x}\end{bmatrix}S_{x}-UV^{\top}-VU^{\top}$
$\displaystyle-S^{\top}\text{He}(\sum_{j=1}^{\alpha}G^{\top}_{\gamma
j}(\tilde{D}_{\gamma j}+\tilde{M}_{\gamma j})G_{\gamma
j}^{k}+\sum_{l=1}^{\beta}G^{\top}_{\eta l}(\tilde{D}_{\eta l}+\tilde{M}_{\eta
l})G_{\eta l}^{k})S,$ $\displaystyle\mathbf{Z}^{q}_{2}$
$\displaystyle=V-U^{q}T,\quad\mathbf{Z}_{3}=T+T^{\top}-Y,$ $\displaystyle
U^{q}$ $\displaystyle=S^{+\top}[S^{\top}_{\lambda}\ G^{\top}_{\gamma 1}\
G_{\gamma 1}^{q\top}\dots\ G^{\top}_{\gamma\alpha}\ G_{\gamma\alpha}^{q\top}\
G^{\top}_{\eta 1}\ G_{\eta 1}^{q\top}\dots G^{\top}_{\eta\beta}\
G_{\eta\beta}^{q\top}],$ $\displaystyle Y$
$\displaystyle=\text{blkdiag}(P^{*},Y_{\gamma
1},\dots,Y_{\gamma\alpha},Y_{\eta 1},\dots,Y_{\eta\beta}),$ $\displaystyle
Y_{\gamma j}$ $\displaystyle=\begin{bmatrix}\mathbf{0}&D^{+}_{\gamma
j}+M^{+}_{\gamma j}\\\ \star&\mathbf{0}\end{bmatrix},Y_{\eta
j}=\begin{bmatrix}\mathbf{0}&D^{+}_{\eta j}+M^{+}_{\eta j}\\\
\star&\mathbf{0}\end{bmatrix}.$

##  VI Numerical Examples

In this section we illustrate the proposed control synthesis procedure on two
examples: A) an inverted pendulum against a soft wall and B) the human-robot
collaboration scenario from section III. For each example, we obtain the DC
decomposition, verify our assumptions and synthesize feedback policies which
are demonstrated via simulations.

###  VI-A Inverted pendulum against Wall

Figure 1: Inverted pendulum with soft wall.

####  VI-A1 System Model

Consider the inverted pendulum of mass $m$ and length $l$ in Figure 1. The
state of the system at time $t$, $x(t)=[\theta(t)\ \dot{\theta}(t)]^{\top}$,
is described by the deviation $\theta$ of the pendulum from the vertical, and
its angular velocity $\dot{\theta}$. At distance $d(t)$ from the vertical,
there is a soft wall modeled as a spring with spring constant $k(t)$. Both the
spring constant $k(t)$ and distance $d(t)$ are uncertain and time-varying. The
torques acting on the pendulum are $mgl\sin\theta$ (due to gravity), $u(t)$
(control torque) and the moment due to the spring given by
$\lambda(t,x)=l\cos\theta\max\\{0,k(t)(l\sin\theta-d(t))\\}$ in the counter-
clockwise direction. Assuming small deviations ($\sin\theta\approx\theta$) and
using forward-Euler discretization, we can write the system dynamics as the
PWA model

$\displaystyle x(t+1)=$ $\displaystyle\underbrace{\begin{bmatrix}1&dt\\\
dt\frac{g}{l}&1\end{bmatrix}}_{A}x(t)+\underbrace{\begin{bmatrix}0\\\
\frac{dt}{ml^{2}}\end{bmatrix}}_{B}u(t)$ (22)
$\displaystyle~{}~{}~{}-\underbrace{\begin{bmatrix}0\\\
\frac{dt}{ml}\end{bmatrix}\max\\{0,k(t)l\theta(t)-k(t)d(t)\\}}_{\eta(t,x(t))}.$

We assume that the uncertain parameters $\mathbf{W}(t)=[k(t)\ k(t)d(t)]$ lie
in some convex polytope $\mathcal{W}=\text{conv}\\{[k^{i}\
k_{d}^{i}]\\}_{i=1}^{n_{V}}$ as in Assumption 2. We can also assume
$k^{i},k_{d}^{i}\geq 0~{}\forall i=1,\dots,n_{V}$ to represent non-negative
spring constants and distances. Thus $k(t),k(t)d(t)\geq 0$ and so we have
$\eta(t,\mathbf{0})=\max\\{0,-k(t)d(t)\\}\begin{bmatrix}0&\frac{dt}{ml}\end{bmatrix}^{\top}=\mathbf{0}=\gamma(t,\mathbf{0})$
in Assumption 3. From Remark 3, the redefined DC decomposition is simply
$\tilde{\eta}(t,x)=\eta(t,x)$.

For policy synthesis, we construct $\chi(t,x)=[1\ x^{\top}\
\eta_{1}(t,x)^{\top}\eta_{2}(t,x)^{\top}]^{\top}$ where
$\eta_{1}(t,x)=\mathbf{0}$ and $\eta_{2}(t,x)=\max\\{\eta_{1}(t,x),[0\
\frac{dtk(t)}{ml}(l\theta-d(t))]^{\top}\\}$. For the observation model, we
assume that the full state $x(t)$ is observable and that we can also observe
the contact torque $\lambda(t,x)$. Thus, the matrix $C\in\mathbb{R}^{4\times
7}$ in Assumption 4 is given by
$C=\begin{bmatrix}[I_{7}]^{\top}_{1}&[I_{7}]^{\top}_{2}&[I_{7}]^{\top}_{3}&[I_{7}]^{\top}_{7}\end{bmatrix}^{\top}.$
We now proceed to synthesize a control policy using theorem 1 for a specific
realization of this system.

####  VI-A2 Simulations

The various parameters in (22) are given as follows (in SI units): $dt=0.01$,
$m=1$, $l=2$, $g=9.8$, $\mathcal{W}=\text{conv}\\{[10\ 2],[10\ 2.5],[5\ 1],[5\
1.25]\\}$. The state and input constraints are given by
$\mathcal{X}=[-0.5,0.5]\times[-2,1]$, $\mathcal{U}=[-200,200]$.

We use YALMIP with SDPT3 to find a feasible solution to the inequalities of
theorem 1. The synthesized policy is tested on three different initial
conditions $x_{0}=\\{[0.18\ 0.8]^{\top},[0.1\ 1]^{\top},[0.22\ 0]^{\top}\\}$

Figure 2: Deviation of the pendulum from the vertical and angular velocity
$\theta(t)$, $\dot{\theta}(t)$ vs. time $t$ for three different initial
conditions for system (22) in closed-loop with the policy $u^{*}(t,x)$ derived
from theorem 1. Figure 3: Contact torque $\lambda(t,x(t))$ and control input
$u^{*}(t,x(t))$ vs. time $t$ for three different initial conditions for system
(22) in closed-loop with the policy $u^{*}(t,x)$ derived from theorem 1.

####  VI-A3 Discussion

Figures 2,3 show that the system trajectories converge to the origin for all
three initial conditions while satisfying state and input constraints. The
uncertainty description tells us that $d(t)\in[0.2,0.25]$, or
$\dfrac{d(t)}{l}\in[0.1,0.125]$. So the pendulum starts close to the wall in
the considered initial conditions, which is confirmed by the first plot in
Figure 3. The first and last trajectories start with the pendulum in contact
with the wall whereas in the second trajectory, the pendulum bounces off the
wall before converging to the origin. The control signal in the second plot of
Figure 3 has kinks when contact is made or broken.

###  VI-B Human-Robot Collaboration

####  VI-B1 System Model

Figure 4: Robot cooperating with human to transport a payload.

Suppose we have a collaborative setup where a robot is transporting a payload
with assistance from a human as illustrated in figure 4. Denote the position
and velocity of the robot by $x_{R}$ and $v_{R}$ respectively. The velocity
$v_{R}$ is controlled by $u$ via first order discrete-time dynamics. Denote
the position of the payload as $x_{P}$ and let $x(t)=[x_{R}(t)\ v_{R}(t)\
x_{P}(t)]^{\top}$ describe the state of the system at time $t$. The velocity
of the payload is determined by the human’s policy as $\lambda(t,x(t))$ given
by

$\displaystyle\lambda(t,x(t))=$
$\displaystyle\begin{cases}K(t)(x_{R}(t)-x_{P}(t))&c(t)x_{P}(t)>x_{R}(t)\\\
K(t)(c(t)-1)x_{P}(t))&c(t)x_{P}(t)\leq x_{R}(t)\\\ \end{cases}$

where $c(t)\in(0,1],K(t)\geq 0$ are uncertain parameters of the human’s
policy. The contact model between the robot and the payload is given by that
of inelastic collision. The model of the system is thus given by

$\displaystyle\begin{bmatrix}x_{R}(t+1)\\\ v_{R}(t+1)\\\
x_{P}(t+1)\end{bmatrix}=\begin{bmatrix}x_{R}(t)+dt\cdot v_{R}(t)\\\
v_{R}(t)+dt\cdot u(t)\\\ \max\\{x_{R}(t)+dt\cdot
v_{R}(t),x_{P}(t)+dt\cdot\lambda(t,x(t))\\}\end{bmatrix}$ (23)

where $dt$ is the discretization time-step. Observe that because $K(t),c(t)$
are uncertain and time-varying, the polytopic partition of the PWA system (23)
is uncertain and time-varying too. Also notice that the set
$\mathcal{X}_{I}=\\{x\in\mathbb{R}^{3}:x_{R}\leq x_{P}\\}$ is invariant for
the dynamics (23) and we assume that $x(0)\in\mathcal{X}_{I}$. When
$x_{p}(t)\geq 0$, the human drives the payload towards the origin if it is
close to the robot ($c(t)x_{P}(t)\leq x_{R}(t)$) and towards the robot
otherwise. When $x_{P}(t)<0$, the payload is always pushed to the left
(towards $-\infty$). Now we derive the DC decomposition of the system model
(23). The velocity that the human imparts to the payload, $\lambda(t,x)$, can
be written as

$\displaystyle\lambda(t,x)=$
$\displaystyle-\max\\{-K(t)(x_{R}-x_{P}),-K(t)(c(t)-1)x_{P}\\}.$

Using this expression and the fact that $\max\\{a,b+c\\}=\max\\{a-b,c\\}+b$,
the system (23) can be written in the form (2) as

$\displaystyle x(t+1)=$ $\displaystyle\underbrace{\begin{bmatrix}1&dt&0\\\
0&1&0\\\ 0&0&0\end{bmatrix}}_{A}x(t)+\underbrace{\begin{bmatrix}0\\\ dt\\\
0\end{bmatrix}}_{B}u(t)+\gamma(t,x(t))-\eta(t,x(t))$ (24)

where $[\gamma(t,x)]_{i}=[\eta(t,x)]_{i}=0$ for $i=1,2$ and
$[\gamma(t,x)]_{3}=\max\\{x_{R}+dt(v_{R}-\lambda(t,x)),x_{P}\\}=\max\\{x_{R}+dt(v_{R}-K(t)(x_{R}-x_{P})),x_{R}+dt(v_{R}-K(t)(c(t)-1)x_{P}),x_{P}\\}$,
$[\eta(t,x)]_{3}=-dt\lambda(t,x)$.

We assume that the uncertain parameters $\mathbf{W}(t)=[K(t)\ K(t)c(t)]$ lie
in some convex polytope $\mathcal{W}=\text{conv}\\{[K^{i}\
K_{C}^{i}]\\}_{i=1}^{n_{V}}$ as in Assumption 2. We also assume that
$K^{i},K_{c}^{i}\geq 0,K_{c}^{i}<K^{i}~{}\forall i=1,\dots,n_{V}$ and so
$K(t)\geq 0,c(t)\in[0,1)$. For any $\mathbf{W}(t)\in\mathcal{W}$, we have
$\gamma(t,\mathbf{0})=\eta(t,\mathbf{0})=\mathbf{0}$. So from Remark 3, the
redefined DC decomposition is simply $\tilde{\gamma}(t,x)=\gamma(t,x)$,
$\tilde{\eta}(t,x)=\eta(t,x)$.

For policy synthesis, we construct $\chi(t,x)=[1\ x^{\top}\
\gamma_{1}(t,x)^{\top}\ \gamma_{2}(t,x)^{\top}\
\gamma_{3}(t,x)^{\top}\eta_{1}(t,x)^{\top}\eta_{2}(t,x)^{\top}]^{\top}$ where
$\gamma_{1}(t,x)=[0\ 0\ x_{R}+dt(v_{R}-K(t)(x_{R}-x_{P}))]^{\top}$,
$\gamma_{2}(t,x)=\max\\{\gamma_{1}(t,x),[0\ 0\
x_{R}+dt(v_{R}-K(t)(c(t)-1)x_{P})]^{\top}\\}$,
$\gamma_{3}(t,x)=\max\\{\gamma_{2}(t,x),[0\ 0\ x_{P}]^{\top}\\}$,
$\eta_{1}(t,x)=[0\ 0\ -dtK(t)(x_{R}-x_{P})]^{\top}$ and
$\eta_{2}(t,x)=\max\\{\eta_{1}(t,x),[0\ 0\ -dtK(t)(c(t)-1)x_{P}]^{\top}\\}$.
For the observation model, we assume that the full state $x(t)$ is observable
and that we can also observe the velocity imparted by the human,
$\lambda(t,x)$. Thus, the matrix $C\in\mathbb{R}^{6\times 19}$ in Assumption 4
is given by
$C=\begin{bmatrix}[I_{19}]^{\top}_{1}&[I_{19}]^{\top}_{2}&[I_{19}]^{\top}_{3}&[I_{19}]^{\top}_{4}&[I_{19}]^{\top}_{13}&[I_{19}]^{\top}_{19}\end{bmatrix}^{\top}$
where the last two rows correspond to $[\gamma_{3}(t,x)]_{3}$ and
$[\eta_{3}(t,x)]_{3}$. We now proceed to synthesize a control policy using
theorem 1 for a specific realization of this system.

####  VI-B2 Simulations

Figure 5: Closed-loop system behaviour of (23) for under policy $u^{*}(t,x)$
derived from theorem 1. The plots on the first row correspond to the initial
condition $x_{0}=[1\ 0\ 3.5]^{\top}$ and the plots on the second row
correspond to the initial condition $x_{0}=[-3\ -2\ 1.5]^{\top}$.

The various parameters in (24) are given as follows (in SI units): $dt=0.01$,
$\mathcal{W}=\text{conv}\\{[10\ 6],[5\ 3],[10\ 8],[5\ 4]\\}$.

We use YALMIP with SDPT3 to find a feasible solution to the inequalities of
Theorem 1. The synthesized policy is tested on two different initial
conditions $x_{0}=\\{[1\ 0\ 3.5]^{\top},[-3\ -2\ 1.5]^{\top}\\}$ in
$\mathcal{X}_{I}$. The two rows of plots in Figure 5 correspond to simulations
with the first and second initial conditions respectively.

####  VI-B3 Discussion

Figure 5 shows that the system trajectories converge to the origin for both
initial conditions. Since $K(t)\geq 0$, $c(t)\in[0,1)$ and
$x(t)\in\mathcal{X}_{I}~{}\forall t\geq 0$, we see that the velocity imparted
by the human $\lambda(t,x(t))$ is always negative. In the first row of plots
for initial condition $x_{0}=[1\ 0\ 3.5]^{\top}$, we see that the robot is
driven to the origin first while the payload follows behind. For the second
initial condition, the robot starts at $x_{R}=-3$. The payload is pushed until
it makes contact with the robot and then the robot pushes it to the origin
with no further action from the human ($\lambda(t,x(t))=0$).

##  VII Conclusion

In this work, we analysed and designed feedback controllers for discrete-time
Piecewise-Affine (PWA) systems with uncertainty in both the affine dynamics
and the polytopic partition. Using the Difference-of-Convex (DC) decomposition
of continuous PWA systems and the lifting technique discussed in section IV,
we provided LMI tests to synthesize quadratic Lyapunov functions and
stabilizing affine policies in a higher dimensional space. When projected back
to the state space, these resulted in time-varying PWQ Lyapunov functions and
time-varying PWA feedback policies. We demonstrated our technique on two
examples- an inverted pendulum against a soft wall and a human-robot
collaboration task to transport a payload.

## References

  * [1] E. Sontag, “Nonlinear regulation: The piecewise linear approach,” IEEE Transactions on automatic control, vol. 26, no. 2, pp. 346–358, 1981.
  * [2] W. P. Heemels, B. De Schutter, and A. Bemporad, “Equivalence of hybrid dynamical models,” Automatica, vol. 37, no. 7, pp. 1085–1091, 2001.
  * [3] A. B. Hempel, P. J. Goulart, and J. Lygeros, “Every continuous piecewise affine function can be obtained by solving a parametric linear program,” in 2013 European Control Conference (ECC), pp. 2657–2662, IEEE, 2013.
  * [4] G. Ferrari-Trecate, F. A. Cuzzola, D. Mignone, and M. Morari, “Analysis of discrete-time piecewise affine and hybrid systems,” Automatica, vol. 38, no. 12, pp. 2139–2146, 2002.
  * [5] D. Mignone, G. Ferrari-Trecate, and M. Morari, “Stability and stabilization of piecewise affine and hybrid systems: An lmi approach,” in Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No. 00CH37187), vol. 1, pp. 504–509, IEEE, 2000.
  * [6] M. Hovd and S. Olaru, “Relaxing pwq lyapunov stability criteria for pwa systems,” Automatica, vol. 49, no. 2, pp. 667–670, 2013.
  * [7] M. Rubagotti, S. Trimboli, D. Bernardini, and A. Bemporad, “Stability and invariance analysis of approximate explicit mpc based on pwa lyapunov functions,” IFAC Proceedings Volumes, vol. 44, no. 1, pp. 5712–5717, 2011\.
  * [8] S. Prajna and A. Papachristodoulou, “Analysis of switched and hybrid systems-beyond piecewise quadratic methods,” in Proceedings of the 2003 American Control Conference, 2003., vol. 4, pp. 2779–2784, IEEE, 2003.
  * [9] A. Bemporad, F. Borrelli, and M. Morari, “Optimal controllers for hybrid systems: Stability and piecewise linear explicit form,” in Proceedings of the 39th IEEE Conference on Decision and Control, vol. 2, pp. 1810–1815, IEEE, 2000.
  * [10] F. Borrelli, M. Baotić, A. Bemporad, and M. Morari, “Dynamic programming for constrained optimal control of discrete-time linear hybrid systems,” Automatica, vol. 41, no. 10, pp. 1709–1721, 2005.
  * [11] E. C. Kerrigan and D. Q. Mayne, “Optimal control of constrained, piecewise affine systems with bounded disturbances,” in Proceedings of the 41st IEEE Conference on Decision and Control, 2002., vol. 2, pp. 1552–1557, IEEE, 2002.
  * [12] Y. Gao, Z. Liu, and H. Chen, “Robust h-$\infty$ control for constrained discrete-time piecewise affine systems with time-varying parametric uncertainties,” IET control theory & applications, vol. 3, no. 8, pp. 1132–1144, 2009.
  * [13] S. Trimboli, M. Rubagotti, and A. Bemporad, “Stability and invariance analysis of uncertain pwa systems based on linear programming,” in 2011 50th IEEE Conference on Decision and Control and European Control Conference, pp. 7398–7403, IEEE, 2011.
  * [14] M. Hovd and S. Olaru, “Parameter-dependent pwq lyapunov function stability criteria for uncertain piecewise linear systems,” 2018.
  * [15] A. B. Hempel, P. J. Goulart, and J. Lygeros, “Inverse parametric optimization with an application to hybrid system control,” IEEE Transactions on automatic control, vol. 60, no. 4, pp. 1064–1069, 2014.
  * [16] L. B. Groff, G. Valmórbida, and J. M. G. da Silva, “Stability analysis of piecewise affine discrete-time systems,” in 2019 IEEE 58th Conference on Decision and Control (CDC), pp. 8172–8177, IEEE, 2019.
  * [17] A. Aydinoglu, V. M. Preciado, and M. Posa, “Contact-aware controller design for complementarity systems,” arXiv preprint arXiv:1909.11221, 2019.
  * [18] R. J. Caverly and J. R. Forbes, “Lmi properties and applications in systems, stability, and control theory,” arXiv preprint arXiv:1903.08599, 2019.


# Efficient Incremental Penetration Depth Estimation
between Convex Geometries

Wei Gao Mech-Mind Robotics. Email<EMAIL_ADDRESS>
###### Abstract

Penetration depth (PD) is essential for robotics due to its extensive
applications in dynamic simulation, motion planning, haptic rendering, etc.
The Expanding Polytope Algorithm (EPA) is the de facto standard for this
problem, which estimates PD by expanding an inner polyhedral approximation of
an implicit set. In this paper, we propose a novel optimization-based
algorithm that incrementally estimates minimum penetration depth and its
direction. One major advantage of our method is that it can be warm-started by
exploiting the spatial and temporal coherence, which emerges naturally in many
robotic applications (e.g., the temporal coherence between adjacent simulation
time knots). As a result, our algorithm achieves substantial speedup –- we
demonstrate it is 5-30x faster than EPA on several benchmarks. Moreover, our
approach is built upon the same implicit geometry representation as EPA, which
enables easy integration and deployment into existing software stacks. We also
provide an open-source implementation for further evaluations and experiments.

##  I Introduction

Penetration depth (PD) is a distance measure that characterizes how much two
overlapping shapes penetrate into each other. This distance measure is of
significant importance to various robotic applications. For instance, 1) in
dynamic simulations, PD is used to calculate the contact force [20, 15, 18] in
nearly all rigid body contact models; 2) in motion planning, many planners
[19, 17, 9] are designed to regulate (minimize) the PD to alleviate and avoid
collisions; and 3) in haptic rendering [7, 13, 11], PD is used to resolve the
interactions between objects. PD estimation is usually coupled with binary
collision detection, which provides overlapping shape pairs as the input [2,
16]. A common strategy to accelerate collision detection and PD estimation is
to split the computation into two phases, as described in [4]. The first phase
is the broad phase which eliminates shape pairs that are too distant. The
second “narrow” phase checks if the shape pairs passed the broad phase are
really colliding, and computes the PD for the colliding pairs. This paper
focuses on PD estimation during the narrow phase, as detailed below.

Problem Formulation.  We investigate two closed convex shapes
$A_{1},A_{2}\subset$ $R^{n}$. Usually, the space dimension $n=2$ or $n=3$,
corresponds to 2-dimensional (2D) or 3-dimensional (3D) setups. Non-convex
shapes can be handled by computing their convex-hull [14] or performing convex
decomposition [12]. Two shapes $A_{1},A_{2}$ collide with each other if
$A_{1}\cap A_{2}\neq\emptyset$. For two shapes $A_{1},A_{2}$ that are in
collision, the penetration depth $\text{PD}(A_{1},A_{2})$ between $A_{1}$ and
$A_{2}$ can be define as

$\displaystyle\begin{split}\text{PD}(A_{1},A_{2})=~{}&\text{min}_{~{}d_{1,2}\in
R^{n}}~{}~{}~{}||d_{1,2}||_{2}\\\ &\text{subject
to:}~{}~{}\text{interior}(A_{1}+d_{1,2})\cap A_{2}=\emptyset\end{split}$ (1)

where $\|\cdot\|_{2}$ is Euclidian norm in $R^{n}$; $A_{1}+d_{1,2}$ is the set
obtained by applying a displacement $d_{1,2}\in R^{n}$ to each point in
$A_{1}$. The displacement $d_{1,2}^{*}\in R^{n}$ that is an optimal solution
to Problem (1) is denoted as minimum penetration displacement. The unit vector
$n_{1,2}^{*}=\text{normalized}(d_{1,2}^{*})$ is usually denoted as minimum
penetration direction. Obviously,
$d_{1,2}^{*}=n_{1,2}^{*}\times\text{PD}(A_{1},A_{2})$. An illustration is
shown in Fig 1.

Figure 1:  An illustration of penetration depth and Minkowski Difference. (a)
For two colliding shapes, their Minkowski Difference contains the origin $O$.
The penetration depth PD$(A_{1},A_{2})$ is also the minimum distance from the
origin $O$ to the boundary of their Minkowski Difference. (b) For two non-
overlapping shapes, the origin $O$ is outside their Minkowski Difference and
the penetration depth is not defined.

Related Works.  The most widely used method for PD estimation is the EPA
algorithm [21], which is tightly coupled with the Gilbert-Johnson-Keerthi
(GJK) algorithm [6] for collision detection. Both GJK and EPA operate on
Minkowski Difference [6], which is a convex set in $R^{2}$ or $R^{3}$
constructed from the original shapes $A_{1}$ and $A_{2}$ (a detailed
explanation of Minkowski Difference is presented in Sec. II). Intuitively, the
EPA algorithm estimates the PD by building an inner convex polyhedral
approximation of the Minkowski Difference. The EPA algorithm requires a
tetrahedron inscribed to the Minkowski Difference as the input, which is
produced by the GJK algorithm for each colliding shape pair $A_{1}$ and
$A_{2}$. By using the Minkowski Difference formulation, GJK and EPA can handle
many different convex shapes such as convex polyhedra and basic primitives
(i.e., spheres, boxes, cylinders etc.). These desirable properties make EPA
the de facto standard for PD estimation. On the other hand, an accurate PD
estimation using EPA may require an inner polyhedral approximation with a lot
of vertices and faces. In this paper, we take a different approach that
incrementally estimates PD by solving an optimization problem. A key advantage
of our method is to leverage spatial and temporal coherence in many robotic
applications to warm-start the optimization procedure. For instance, in
dynamic simulation the object pairs tend to have very similar penetration
direction and depth in consecutive simulation steps. Initializing the proposed
PD algorithm by the penetration direction of the previous time knot might lead
to substantial speed up.

The idea of incrementally estimating the PD has also been explored in [10],
which seeks a locally optimal solution of PD by walking on vertices of Gauss
maps. However, [10] is restricted to convex polyhedra. As a result, [10]
cannot handle general convex geometries and the integration into existing
software stacks [2, 16] can be hard. Researchers have also proposed to
explicitly construct the Minkowski Difference [3] for collision detection and
PD estimation. However, the construction procedures tend to be expensive in
terms of memory and computation.

Contributions.  Built upon the seminal works [6, 21, 8], this paper proposes a
novel PD estimation algorithm between general convex shapes. In particular,

  * •

We formulate PD estimation as a “Difference-of-Convex” problem, which is
usually solved by Sequential Quadratic Programming (SQP) [1]. The major
challenge of applying SQP to PD estimation is the implicit geometry
representation (the Support Function explained in Sec. II-B). To address it,
we propose a novel instantiation of SQP that utilizes a modified Minkowski
Portal Refinement (MPR) algorithm [8] as a subroutine.

  * •

We propose a novel shortcut mechanism that reduces the computation of the
vanilla SQP procedure for PD estimation. Furthermore, we demonstrate that the
algorithm can still converge to locally optimal solutions despite
incorporating the shortcut.

  * •

We experimentally evaluate our method on several benchmarks. Our method
demonstrates a 5-30x speedup over EPA at a comparable accuracy.

  * •

We provide an open-source implementation to reproduce the proposed algorithm
and facilitate its usage in other robotic applications.

This paper is organized as follows: in Sec. II we introduce the preliminaries
about the support function, Minkowski Difference, and MPR algorithm. Sec. III
introduces the optimization-based PD algorithm and the shortcut mechanism.
Sec. IV presents the experimental results and comparison with the state-of-
the-art. Sec. V concludes.

##  II Preliminary

###  II-A Minkowski Difference and Penetration Depth

As mentioned in Sec. I, we investigate two closed, convex shapes
$A_{1},A_{2}\subset$ $R^{n}$. Usually the space dimension $n=2$ or $n=3$. The
Minkowski Difference $\mathcal{D}_{1,2}$ for shapes $A_{1}$ and $A_{2}$ is
defined as

$\displaystyle\begin{split}\mathcal{D}_{1,2}=A_{1}-A_{2}=\\{v=v_{1}-v_{2}|v_{1}\in
A_{1},v_{2}\in A_{2}\\}\end{split}$ (2)

The following properties hold for the Minkowski Difference
$\mathcal{D}_{1,2}$:

  * •

$\mathcal{D}_{1,2}$ is a closed convex set if both $A_{1}$ and $A_{2}$ are
closed convex sets.

  * •

Two shapes $A_{1}$ and $A_{2}$ overlap with each other if the origin
$O\in\mathcal{D}_{1,2}$.

  * •

For two overlapping shapes $A_{1}$ and $A_{2}$, their penetration depth
$\text{PD}(A_{1},A_{2})$ defined in Problem (1) corresponds to the minimum
distance from $O$ to the boundary of $\mathcal{D}_{1,2}$, as proved in [21].

An illustration of the Minkowski Difference is provided in Fig. 1. Using these
properties, we can reformulate Problem (1) in Sec. I into the following new
optimization problem

$\displaystyle\begin{split}\text{PD}(A_{1},A_{2})=~{}&\text{min}_{~{}v\in
R^{n}}~{}~{}~{}||v||_{2}\\\ &\text{subject
to:}~{}~{}v\in\text{boundary}(\mathcal{D}_{1,2})\end{split}$ (3)

Let the point $v^{*}$ on the boundary of $\mathcal{D}_{1,2}$ be one optimal
solution to Problem (4). The minimum penetration displacement $d_{1,2}^{*}$ in
Sec. I can be computed as $d_{1,2}^{*}=-v^{*}$. In other words, we have

$\displaystyle\begin{split}\text{interior}(A_{1}-v^{*})\cap
A_{2}=\emptyset\end{split}$ (4)

The minimum penetration direction in Sec. I can be computed as
$n_{1,2}^{*}=\text{normalized}(-v^{*})$.

###  II-B Support Function for Convex Shapes

Figure 2:  An illustration of a modified MPR algorithm to compute the
intersection point (the $v_{\text{intersect}}$ in (a)) of an origin ray with
the boundary of the Minkowski Difference. The algorithm takes the ray
direction $d_{\text{ray}}$ as the input. In each iteration, the algorithm
maintains and updates a “portal”, which is a segment for 2D (or a triangle for
3D) that is inscribed to the Minkowski Difference. The illustrations from (a)
to (c) show one MPR iteration, as the portal $(v_{1},v_{2})$ in (a) is updated
to a new portal $(v_{2},v_{\text{candidate}})$ in (c). The detailed procedure
is explained in Sec. II-C.

Explicitly constructing the Minkowski Difference $\mathcal{D}_{1,2}$ can be
computationally expensive. Thus, [6] proposed to use an implicit, functional
geometry representation called support function. For a given convex set $A$,
we can define its support function $\text{supp}_{A}(\cdot)$ as

$\displaystyle\begin{split}\text{supp}_{A}(d)=\text{argmin}_{~{}x\in
A}~{}~{}\text{dot}(x,d)~{}\subset A\end{split}$ (5)

where $\text{dot}(\cdot,\cdot)$ is the dot product in $R^{n}$, and $d$ is a
unit vector denoted as support direction. Intuitively, the support function
computes points in $A$ that are the farthest along the support direction. For
some direction $d$, there might be more than one point that satisfies the
definition in Equ. (5). On the other hand, algorithms in this paper and
previous works [6, 21, 8] only need (arbitrary) one such point. Thus, in the
following text we assume that the support function $\text{supp}_{A}(\cdot)$
returns one point for each direction.

The support function is closely related to the supporting hyperplane [1] of a
convex set. For a given direction $d$ and $v=\text{supp}_{A}(d)$, the plane
$\\{x|\text{dot}(d,x-v)=0\\}$ is a supporting plane for the convex shape $A$
at the point $v\in A$. Geometrically, this supporting plane has normal
direction $d$ and passes through a point $v\in A$. The convex set $A$ is
contained in one of the two closed half-spaces defined by the supporting
hyperplane.

As shown in [6], we can use the support functions $\text{supp}_{A_{1}}(\cdot)$
and $\text{supp}_{A_{2}}(\cdot)$ of two shapes $A_{1},~{}A_{2}$ to construct
the support function of their Minkowski Difference
$\text{supp}_{\mathcal{D}_{1,2}}(\cdot)$. In particular,

$\text{supp}_{\mathcal{D}_{1,2}}(d)=\text{supp}_{A_{1}}(d)-\text{supp}_{A_{2}}(-d)$
(6)

In other words, we can get a point from $\text{supp}_{\mathcal{D}_{1,2}}(d)$
using the two points from $\text{supp}_{A_{1}}(d)$ and
$\text{supp}_{A_{2}}(-d)$, respectively. The algorithm presented in this work
will access the Minkowski Difference ${\mathcal{D}_{1,2}}$ through its support
function, instead of explicitly constructing it.

###  II-C Minkowski Portal Refinement Algorithm

The MPR [8] algorithm was originally proposed for binary collision checking,
in other words determining whether the origin $O$ is contained in the
Minkowski Difference ${\mathcal{D}_{1,2}}$. A brief introduction to a modified
MPR is presented, which is used as a subroutine for the algorithm in Sec. III.
Please refer to Chapter 7 of [8] for a complete description.

We investigate two overlapping shapes $A_{1}$ and $A_{2}$, thus their
Minkowski Difference ${\mathcal{D}_{1,2}}$ contains the origin. Given a
direction $d_{\text{ray}}$ as the input, we define the origin ray that starts
from the origin $O$ and extends along $d_{\text{ray}}$

$\displaystyle\begin{split}\text{origin\\_ray}(d_{\text{ray}})=\\{x|x=t~{}d_{\text{ray}},\text{~{}for~{}}t\geq
0\\}\end{split}$ (7)

We would present a modified MPR algorithm that computes the intersection point
of the origin ray with the boundary of Minkowski Difference
${\mathcal{D}_{1,2}}$, defined as

$\displaystyle\begin{split}v_{\text{intersect}}(d_{\text{ray}})=\text{origin\\_ray}(d_{\text{ray}})\cap\text{boundary}({\mathcal{D}_{1,2}})\end{split}$
(8)

An illustration is provided in Fig. 2 (a), and the algorithm is presented in
Algorithm 1.

The MPR algorithm maintains and updates a portal. As defined in [8], a portal
is a $n$-simplex inscribed to the Minkowski Difference
${\mathcal{D}_{1,2}}\subset R^{n}$ that intersects with the origin ray. For 2D
setup ($n=2$) the portal is a segment inscribed to ${\mathcal{D}_{1,2}}$. For
example, the segment $(v_{1},v_{2})$ in Fig. 2 (a) is a valid portal that
intersects the origin ray at $p$. In 3D setup ($n=3$) the portal is a triangle
inscribed to ${\mathcal{D}_{1,2}}$. At the beginning of the algorithm, MPR
proposed a “find_portal” procedure [8] to discover a valid portal, as shown in
Algorithm 1.

After initialization, MPR first computes the normal of the portal
$d_{\text{portal}}$ in each iteration, as shown in Fig. 2 and Algorithm 1.
Then, the support function is invoked with the portal normal to compute a
candidate point
$v_{\text{candidate}}=\text{supp}_{\mathcal{D}_{1,2}}(d_{\text{portal}})$. As
the origin ray intersects with portal $(v_{1},v_{2})$, the origin ray must
intersect with at least one of the segment $(v_{1},v_{\text{candidate}})$ and
$(v_{2},v_{\text{candidate}})$. The intersecting one will be selected as the
new portal for the next iteration. This is the update_portal precedure in
Algorithm 1, and the updated portal is $(v_{2},v_{\text{candidate}})$ in Fig.
2. The iteration for 3D MPR using triangle portals is conceptually similar,
and please refer to Chapter 7 of [8] for a detailed explanation.

It is easy to show the norm $|p|\leq|v_{\text{intersect}}|\leq|q|$, where
$p,~{}q$ are the intersecting point of the origin ray with the portal and the
supporting hyperplane, as shown in Fig. 2 (b). Thus, one termination condition
is to stop the algorithm when the distance between the portal and the
supporting hyperplane (at $v_{\text{candidate}}$) is smaller than some
tolerance $\Delta$, which can be expressed as
$|\text{dot}(q-p,~{}\text{normalized}(d_{\text{portal}}))|\leq\Delta$.
Algorithm 1 returns several terms upon convergence, their usage will be
introduced in Sec. III-B.

Algorithm 1 MPR Algorithm in Sec. II-C

$\mathcal{D}_{1,2}$ with support function
$\text{supp}_{\mathcal{D}_{1,2}}(\cdot)$

ray direction $d_{\text{ray}}$

tolerance $\Delta$

$\text{portal}_{0}\leftarrow\text{find\\_portal}(\mathcal{D}_{1,2})$

while $k=0,1,2,...$ do

$d_{\text{portal}\\_k}\leftarrow\text{portal\\_normal}(\text{portal}_{k})$

$v_{\text{candidate}}\leftarrow\text{supp}_{\mathcal{D}_{1,2}}(\text{portal}_{k})$

$p_{k}\leftarrow\text{intersect}(\text{origin\\_ray}(d_{\text{ray}}),d_{\text{portal}\\_k})$

$\triangleright$ A plane is defined by its normal and one point on it

$q_{k}\leftarrow\text{intersect}(\text{origin\\_ray}(d_{\text{ray}}),\text{Plane}(d_{\text{portal}\\_k},v_{\text{candidate}}))$

if
$|\text{dot}(q_{k}-p_{k},\text{normalized}(d_{\text{portal}\\_k}))|\leq\Delta$
then

return ($p_{k}$, $v_{\text{candidate}}$, $d_{\text{portal}\\_k}$)

end if

$\text{portal}_{k+1}\leftarrow\text{update\\_portal}(\text{portal}_{k},~{}v_{\text{candidate}})$

end while

##  III Penetration Depth Estimation Algorithm

From the analysis in Sec. II, PD estimation can be expressed in the following
optimization problem

$\displaystyle\begin{split}&\text{min}_{~{}v\in R^{n}}~{}~{}~{}||v||_{2}\\\
&\text{subject to:}~{}~{}v\in\text{boundary}(\mathcal{D})\end{split}$ (9)

where $\mathcal{D}$ is the Minkowski Difference. Here we drop the subscript of
${\mathcal{D}_{1,2}}$ for notational simplicity. From the analysis in Sec. II,
$\mathcal{D}$ is a closed convex set that contains the origin.

As explained in Sec. II-A, we can only access the convex set $\mathcal{D}$
through its support function $\text{supp}_{\mathcal{D}}(\cdot)$. This implicit
geometric representation implies:

  1. 1.

For a given point $v\in\text{boundary}(\mathcal{D})$, it is hard to compute a
supporting hyperplane of $\mathcal{D}$ (or equivalently the hyperplane normal)
that passes through $v$. This operation is required for many optimization
procedures, such as the SQP in Sec. III-A.

  2. 2.

The support function $\text{supp}_{\mathcal{D}}(\cdot)$ is actually an
“inverse” of the operation in 1): for a given normal direction $d$, we can
compute a point $v=\text{supp}_{\mathcal{D}}(d)$. This point, combined with
the normal $d$, is able to define a supporting hyperplane for $\mathcal{D}$.

In this section we will address this challenge and propose an optimization-
based PD estimation algorithm.

To make a clear presentation, we will first introduce two “conceptual”
algorithms in Sec. III-A and Sec. III-B. The first one highlights the
Difference-of-Convex formulation and SQP procedure while assuming we can
explicitly compute a supporting hyperplane for any vertex
$v\in\text{boundary}(\mathcal{D})$. As mentioned above, this assumption is not
true for the Minkowski Difference. To address it, we propose a novel
instantiation of SQP that utilizes a modified MPR algorithm as a subroutine.
This is the second “conceptual” algorithm in Sec. III-B. Finally, in Sec.
III-C we present the actual PD algorithm with a shortcut mechanism. This
shortcut can reduce the computation and eliminate the intermediate tolerance
parameter for the MPR subroutine in Sec. III-B.

###  III-A Formulation as a Difference-of-Convex Problem

In this subsection, we investigate the following optimization problem, which
is very similar to Problem (LABEL:equ:actualopt):

$\displaystyle\begin{split}&\text{min}_{~{}v\in R^{n}}~{}~{}~{}||v||_{2}\\\
&\text{subject to:}~{}~{}v\in\text{boundary}(\mathcal{E})\end{split}$ (10)

where $\mathcal{E}$ is also a closed convex set in $R^{n}$ that contains the
origin. Different from Problem (LABEL:equ:actualopt), we assume an explicit
representation of $\mathcal{E}$ is available, such that we can compute a
supporting hyperplane for each point $v\in\text{boundary}(\mathcal{E})$. As
this assumption does not hold for the Minkowski Difference $\mathcal{D}$, the
PD algorithm presented in this subsection is “conceptual” and will be
instantiated in the following subsections.

As a concrete example, the convex shape $\mathcal{E}$ might be represented as
the level-set of a continuous convex function $g(v)$. In other words,
$\mathcal{E}=\\{v|g(v)\leq 0\\}$ and
$\text{boundary}(\mathcal{E})=\\{v|g(v)=0\\}$. For a point
$v\in\text{boundary}(\mathcal{E})$, we can compute a supporting hyperplane
whose normal is $-\nabla g(v)$, where $\nabla$ is the subgradient operator.

The domain for the decision variable in Problem (LABEL:equ:concaveopt) only
includes the boundary of $\mathcal{E}$. As the convex set $\mathcal{E}$
contains the origin, we can enlarge the input domain to everywhere except the
inner of $\mathcal{E}$ and rewrite the optimization as

$\displaystyle\begin{split}&\text{min}_{~{}v\in R^{n}}~{}~{}~{}||v||_{2}\\\
&\text{subject
to:}~{}~{}v\in(R^{n}\setminus\text{inner}(\mathcal{E}))\end{split}$ (11)

where $R^{n}\setminus\text{inner}(\mathcal{E})$ is the complement of
$\text{inner}(\mathcal{E})$. Problem (LABEL:equ:concaveoptexternal) can be
solved using the Sequential Quadratic Programming (SQP) [1] algorithm, which
alternates between linearization of constraints and internal Quadratic
Programming (QP) sub-steps. Moreover, Problem (LABEL:equ:concaveoptexternal)
is actually a Difference-of-Convex problem. Thus, several additional
properties can be used to simplify the SQP:

  * •

SQP usually requires a trust region. This trust region can be infinity for
Difference-of-Convex problems [1].

  * •

The linearization of the constraint
$v\in(R^{n}\setminus\text{inner}(\mathcal{E}))$ is a halfspace outside the
supporting hyperplane. Thus, the inner QP step can be implemented by
projecting the origin onto the supporting hyperplane.

The SQP algorithm is presented in Algorithm 2, and the detailed derivation is
provided in the Supplemental Material.

Algorithm 2 SQP for Problem. LABEL:equ:concaveoptexternal

$\mathcal{E}$ that supports compute_supporting_hyperplane($\cdot$)

$v_{\text{init}}\in\text{boundary}(\mathcal{E})$

$v_{0}\leftarrow v_{\text{init}}$

while $k=0,1,2,...$ do

$n_{\text{plane}\\_k}\leftarrow\text{compute\\_supporting\\_hyperplane}(\mathcal{E},~{}v_{k})$

$\triangleright$ Plane defined by normal $n_{\text{plane}\\_k}$ and a point
$v_{k}$ on it

$y_{k}\leftarrow\text{project\\_point\\_to\\_plane}($O$,~{}\text{Plane}(v_{k},~{}n_{\text{plane}\\_k}))$

$v_{k+1}=\text{boundary\\_intersection}(O\\_\text{to}\\_y_{k},~{}\text{boundary}(\mathcal{E}))$

end while

return $v_{k}$

As shown in Algorithm 2, the algorithm requires an initial point
$v_{\text{init}}$ on the boundary of $\mathcal{E}$. In each iteration, the
supporting hyperplane (normal) is first computed. Then, the origin $O$ is
projected to the supporting hyperplane to get the point $y_{k}$. The point
$v_{k+1}$ for the next iteration is computed as the intersection of segment
$(O,y_{k})$ with boundary$(\mathcal{E})$. As shown in Supplemental Materials,
$|v_{k}|$ is an upper bound of the penetration depth and it monotonically
decreases with iterations ($|v_{k+1}|\leq|v_{k}|$).

Algorithm 2 cannot be directly applied to Minkowski Difference $\mathcal{D}$,
as it requires explicit geometric representation to compute a supporting
hyperplane for each boundary point. This challenge will be addressed in Sec.
III-B.

###  III-B Instantiation using MPR as a Subroutine

In this subsection, we adopt the SQP procedure in Algorithm 2 to the Minkowski
Difference $\mathcal{D}$, which is represented by its support function
$\text{supp}_{\mathcal{D}}(\cdot)$. For each
$v\in\text{boundary}(\mathcal{D})$, we can compute its direction
$d_{v}=\text{normalized}(v)$ and a scalar norm $|v|$. We propose to use
$d_{v}$ as the decision variable and represent $v$ in Problem
(LABEL:equ:concaveoptexternal) implicitly by $d_{v}$, based on the following
observations:

  1. 1.

The point $v\in\text{boundary}(\mathcal{D})$ can be estimated from $d_{v}$
(with accuracy up on some tolerance $\Delta$), using the MPR algorithm in Sec.
II-C as a subroutine.

  2. 2.

More importantly, the supporting hyperplane normal at $v$ can be approximated
by the portal normal $d_{\text{portal}}$, upon the convergence of the MPR
algorithm. It is emphasized that usually $d_{\text{portal}}\neq d_{v}$.

With these observations, we can propose the SQP procedure in Algorithm 3 for
the Minkowski Difference $\mathcal{D}$. The iteration is similar to the one in
Algorithm 2. As this algorithm will be superseded in Sec. III-C with a new one
with a “shortcut” mechanism, we only provide an intuitive analysis in this
subsection.

A critical property of Algorithm 3 is approximating a supporting hyperplane
normal by $d_{\text{portal}\\_k}$, which is further illustrated in Fig. 3.
Upon the convergence of MPR (Algorithm 1) subroutine with tolerance $\Delta$,
the grounding-truth intersecting point $v$ for direction $d_{v}$ must lie in
the between of the portal (segment
$(v_{\text{portal}\\_1},v_{\text{portal}\\_2})$ in Fig. 3) and the supporting
hyperplane at $v_{\text{candidate}}$. The portal and supporting hyperplane are
parallel to each other (they share the same normal $d_{\text{portal}}$), and
the distance between these two planes is no more than $\Delta$ as the MPR
terminates in this iteration. Algorithm 1 uses $p$ to approximate $v$ and uses
$d_{\text{portal}}$ to approximate a supporting hyperplane normal at $v$. This
approximation is equivalent to removing points from $\mathcal{D}$ that are
separated from the origin by the portal, as illustrated in Fig. 3 (b). This is
because $p$ is the exact intersecting point for the shape in Fig. 3 (b), while
$d_{\text{portal}}$ is the exact supporting hyperplane at that intersecting
point. The removed part is a convex region whose “thickness” along
$d_{\text{portal}}$ is no more than $\Delta$. Thus, the approximation becomes
more accurate as the tolerance $\Delta$ is set smaller.

Despite Algorithm 3 might work in practice, it is not the optimal choice.
There can be many iterations in the MPR subroutine for a small tolerance
$\Delta$, which is computationally expensive. This issue will be addressed in
Sec. III-C.

Figure 3:  Approximation scheme used in Sec. III-B and Algorithm 3. (a) shows
the configuration of the portal and supporting hyperplane upon the convergence
(with tolerance $\Delta$) of MPR, when it is used as a subroutine in Algorithm
3. Intuitively, the approximation in Sec. III-B corresponds to removing all
points “outside” the final portal, as shown in (b). A detailed explanation is
in Sec. III-B.  Algorithm 3 SQP using Support Function

support function $\text{supp}_{\mathcal{D}}(\cdot)$

initial direction $d_{\text{init}}$

tolerance $\Delta$ for MPR subprocedure

$d_{0}\leftarrow d_{\text{init}}$

while $k=0,1,2,...$ do

$\text{mpr\\_output}\leftarrow\text{mpr}(\text{supp}_{\mathcal{D}},~{}d_{k},~{}\Delta)$
$\triangleright$ Alg. 1

$(p_{k},~{}v_{\text{candidate}},~{}d_{\text{portal}\\_k})\leftarrow\text{mpr\\_output}$

$z_{k}\leftarrow\text{project\\_origin\\_to\\_plane}(\text{Plane}(v_{\text{candidate}},~{}d_{\text{portal}\\_k}))$

if should_terminate($p_{k},~{}z_{k},~{}d_{\text{portal}\\_k}$) then

return $z_{k}$ $\triangleright$ Sec. III-D

end if

$d_{k+1}\leftarrow d_{\text{portal}\\_k}$

end while

return $p_{k}$

###  III-C Penetration Depth Estimation with Shortcut

In existing software stacks [16, 2], MPR is typically used with a rather small
tolerance to achieve better accuracy. On the other hand, the primary
functionality for the MPR subroutine in Algorithm 3 is to produce a search
direction $d_{k+1}$ via the supporting hyperplane normal $d_{\text{portal}}$.
Thus, we propose to reduce the MPR subroutine iterations by an early
termination mechanism (shortcut) once a “good” search direction is discovered.

Fig. 4 shows the configuration of the portal and the supporting hyperplane
configuration in the MPR subroutine. The distance between the portal and the
supporting hyperplane might be (much) larger than the tolerance $\Delta$, as
it is not necessarily the final iteration. Let the point $p$ be the
intersection of $\text{origin\\_ray}(d_{v})$ with the portal. MPR subroutine
uses $p$ as the current estimation of $v$ (the intersection with the boundary
of $\mathcal{D}$), and $|p|\leq|v|$. In other words, the PD we would find is
lower bounded by $|p|$ if we continue the MPR subroutine with direction
$d_{v}$.

Besides, we can project origin $O$ to the supporting hyperplane to get $z$, as
shown in Fig 4. Let $v_{z}$ be the intersection of segment $(O,z)$ with the
boundary of $\mathcal{D}$. Obviously $|v_{z}|\leq|z|$. If we switch to a new
search direction $d_{z}=\text{normalized}(z)$, the PD we would find is upper
bounded by $|z|$.

From the analysis above, in each MPR subroutine iteration, we have: 1) a lower
bound of PD $|p|$ along the current search direction $d_{v}$; and 2) a new
search direction $d_{z}$ candidate with an upper bound on PD $|z|$ along it.
We propose to switch to the new search direction $d_{z}$ once the upper bound
along it is smaller than the lower bound along the original direction $d_{v}$,
as shown in Fig. 4.

With the analysis above, the overall PD estimation algorithm is summarized in
Algorithm 4, which uses a subroutine in Algorithm 5. The SQP procedure is
almost identical to Algorithm 3, except that the MPR subroutine is changed to
Algorithm 5. The MPR subroutine would be terminated early with the
abovementioned condition, as shown in Algorithm 5. Despite the new shortcut
mechanism, this SQP is guaranteed to converge to a locally optimal solution.
Please refer to the Supplemental Material for a detailed analysis.

Figure 4:  An illustration of the shortcut mechanism in Sec. III-C. The
penetration depth along $d_{v}$ is lower bounded by $|p|$, while the
penetration depth along $d_{\text{portal}}(d_{z})$ is upper bounded by $z$. We
proposed to switch to $d_{z}$ as the new search direction when $|p|\geq|z|$,
as explained in Sec. III-C.

###  III-D Implementation Details

Initialization. As shown in Algorithm 4, the SQP procedure requires an initial
direction $d_{\text{init}}$ as the input. This is a guess of the smallest
displacement direction that could move shape $A_{1}$ away from colliding with
shape $A_{2}$. For applications that exhibit high spatial or temporal
coherence between successive frames/time-knots, there might be a good
application-specific guess of the minimum penetration direction. For instance,
in dynamic simulation [20, 15, 18] colliding object pairs tend to have very
similar penetration direction and depth in consecutive simulation steps.
Similarly, in optimization-based motion planning [19, 17, 9] we might use the
penetration direction from the previous optimization iteration as
$d_{\text{init}}$, especially for planning algorithms with trust-region
mechanisms to ensure a small difference between consecutive iterations.

If application-specific prior about penetration direction is unavailable, one
alternative method is to use the centroid difference between two shapes as
$d_{\text{init}}$. The original MPR [8] implemented in existing software
stacks [16, 2] also requires the centroid of shapes (although an inner point
is sufficient for the algorithm). Thus, this initialization scheme can be
directly plugged in with minimal engineering effort.

Termination and Result. As the PD is usually used to resolve the collision in
downstream tasks, it is better to overestimate the PD than underestimate it.
Thus, the upper bound $z_{k}$ is returned upon termination in Algorithm 4. A
point $v$ on the boundary of $\mathcal{D}$ is the local optima of $|v|$ if $v$
is perpendicular to one supporting hyperplane at $v$. In this situation, the
MPR subroutine cannot find a new direction. Thus, we would terminate the SQP
in Algorithm 4 if the new search direction $d_{\text{portal}\\_k}$ is the same
as the old one $\text{normalized}(p_{k})$ up to some tolerance. Moreover, the
gap between the lower bound $p_{k}$ and the upper bound $z_{k}$ should be
rather small under this situation, which is also included in the termination
condition.

Algorithm 4 Proposed SQP Algorithm for PD Estimation

support function $\text{supp}_{\mathcal{D}}(\cdot)$

initial direction $d_{\text{init}}$

Replaces the invoked MPR subroutine in Algorithm 3 by mpr_shortcut in
Algorithm 5. The tolerance $\Delta$ in Algorithm 3 is no longer necessary.

Algorithm 5 MPR Subroutine with Shortcut in Sec. III-C

$\mathcal{D}_{1,2}$ with support function
$\text{supp}_{\mathcal{D}_{1,2}}(\cdot)$

ray direction $d_{\text{ray}}$

$\text{portal}_{0}\leftarrow\text{find\\_portal}(\mathcal{D}_{1,2})$

while $k=0,1,2,...$ do

$d_{\text{portal}\\_k}\leftarrow\text{portal\\_normal}(\text{portal}_{k})$

$v_{\text{candidate}}\leftarrow\text{supp}_{\mathcal{D}_{1,2}}(\text{portal}_{k})$

$p_{k}\leftarrow\text{intersect}(\text{origin\\_ray}(d_{\text{ray}}),d_{\text{portal}\\_k})$

$\triangleright$ A plane is defined by its normal and one point on it

$z_{k}\leftarrow\text{project\\_origin\\_to\\_plane}(\text{Plane}(v_{\text{candidate}},~{}d_{\text{portal}\\_k}))$

if $|z_{k}|\leq|p_{k}|$ then $\triangleright$ Shortcut in Sec. III-C

return $p_{k}$, $v_{\text{candidate}}$, $d_{\text{portal}\\_k}$

end if

$\text{portal}_{k+1}\leftarrow\text{update\\_portal}(\text{portal}_{k},~{}v_{\text{candidate}})$

end while

##  IV Results

In this section, we experimentally investigate the robustness, accuracy and
efficiency of the proposed method. Our method is implemented in a forked fcl
[16] library. The proposed method is compared with the EPA [21] algorithm in
libccd [5], a popular library used in various physical engines and collision
detectors [16, 18]. Parameters are tuned such that all algorithms have roughly
the same accuracy and are fixed for the entire experiment after tuning. Please
refer to Supplemental Materials for more details. Our method is evaluated on
two sets of shapes. The first one consists of simple primitives, and the
second set is convex polyhedra with different numbers of vertices. Our
hardware platform is a desktop with AMD 3970x CPU running Ubuntu 20.04. Our
implementation is open-sourced at this link.

###  IV-A Primitive Shapes

In this subsection, we consider the penetration depth between three types of
shape pairs:

  * •

Sphere collides with sphere (sphere vs. sphere)

  * •

Capsule collides with capsule (capsule vs. capsule)

  * •

Sphere collides with capsule (sphere vs. capsule)

For these simple primitive shapes, the fcl [16] library provides hand-written
algorithms to compute their penetration, which is used as ground truth to
evaluate the accuracy of PD algorithms. The geometries have characteristic
dimensions (such as the diameter of the sphere) of 1 meter in this experiment.
For more details, please refer to the Supplemental Materials. All of the
statistical results are the averaged value of 10000 independent runs with
randomly generated poses for the shape pair.

Our method requires an initial penetration direction guess. For the purpose of
benchmarking, we use the following method to obtain the initial guess. Given
the ground truth penetration direction from the hand-written algorithm, we
apply a rotation of $n$ degrees with respect to a random axis that is
perpendicular to the ground-truth direction. Then, the rotated direction is
used as the initial guess.

Table. I shows the accuracy of our method under different rotation
perturbation angles $n$, where $n$ ranges from $5^{\circ}$ to $45^{\circ}$. We
report the average deviation of the estimated PD from the ground truth among
10000 independent runs. From Table I, both EPA and our method can accurately
estimate penetration depth. The results from the proposed algorithm can be
rather accurate despite the large initialization error. Moreover, all the
penetration depths and directions computed by our method lead to the
separation of two shapes, as explained in Equ. 1.

Table. II summarizes the performance of our method and the EPA baseline. Our
method achieves 20x-30x speed up compared to EPA. Moreover, the performance
improvement is consistent for different shape types and deviations of initial
guess direction.

###  IV-B Convex polyhedra

The second experiment considers the penetration detection estimation involving
convex polyhedra. We use the sphere-capsule penetration similar to Sec IV-A,
but replace the sphere with its convex polyhedral approximation. This
approximation is constructed by discretizing the sphere at different
resolutions. Higher resolution with more vertices and faces improves the
approximation accuracy at the cost of computational complexity. Please refer
to the Supplemental Material for more details. The ground truth penetration is
unavailable due to the discretization error.

Fig. 6 shows the performance of the proposed algorithm with respect to the
number of vertices in the convex polyhedra. Our method is evaluated with
different angular deviations (of initial penetration direction) at $5^{\circ}$
and $45^{\circ}$. The proposed method consistently outperforms EPA baselines
with about 5-10x speedup.

A comparative experiment is conducted to highlight the benefit of the proposed
shortcut mechanism in Sec. III-C. We use an initial direction deviation of
$45^{\circ}$, and the result is shown in Fig. 5. The number of support
function invocations and running time are used to characterize the
performance. From the figure, the proposed shortcut mechanism can halve the
required support function invocations and lead to about 2-3x speedup.

|  Ours ($5^{\circ}$) |  Ours ($25^{\circ}$) |  Ours ($45^{\circ}$) |  EPA
---|---|---|---|---
Sphere vs. Sphere |  0.909 |  1.018 |  1.03 |  1.58
Capsule vs. Capsule |  1.092 |  1.12 |  1.12 |  0.39
Sphere vs. Capsule |  1.255 |  1.31 |  1.30 |  1.72
TABLE I:  The PD error of various algorithms compared with the ground truth. The result errors are the average of 10000 independent runs expressed in micrometers ($10^{-6}$ meter). Our method is evaluated with different angular deviations (of initial penetration direction) at $5^{\circ}$, $25^{\circ}$, and $45^{\circ}$. Geometries have characteristic dimensions (such as the diameter of the sphere) of 1 meter.  |  Ours ($5^{\circ}$) |  Ours ($25^{\circ}$) |  Ours ($45^{\circ}$) |  EPA
---|---|---|---|---
Sphere vs. Sphere |  1.9 |  2.1 |  2.3 |  85.1
Capsule vs. Capsule |  1.6 |  1.9 |  2.0 |  39.5
Sphere vs. Capsule |  2.0 |  2.2 |  2.4 |  61.5
TABLE II:  Performance of the proposed method under different angular
deviations of the initial direction guess. Results are the averaged time in
microseconds computed from 10000 independent runs with different colliding
poses. The proposed method is 20x-30x times faster than EPA. Figure 5:  A
comparative experiment is conducted to highlight the benefit of the proposed
shortcut mechanism in Sec. III-C. In the figure, (a) shows the performance
measured by the number of support function invocations, while (b) shows the
performance in microseconds. The proposed shortcut mechanism can halve the
required support function invocations and lead to about 2-3x speedup.  Figure
6:  The performance of various PD algorithms with respect to the number of
vertices in convex polyhedron. Results are the averaged time in microseconds
computed from 10000 independent runs. Our method is evaluated with different
angular deviations (of initial penetration direction) at $5^{\circ}$ and
$45^{\circ}$. The proposed method achieves substantial speedup compared to the
EPA baseline.

##  V Conclusion

In this paper, we contribute a novel algorithm to estimate the PD between
convex shapes. To achieve this, we formulate the PD estimation as a
Difference-of-Convex optimization. Then, we propose a novel instantiation of
SQP using a modified MPR subroutine that solves the optimization-based PD
estimation. We further present a shortcut mechanism that significantly reduces
the computation. Through various experiments, we show that the proposed method
achieves a 5-30x speedup over the well-known EPA algorithm at comparable
accuracy. Moreover, the proposed method uses the same support function
representation as existing works [6, 21, 8]. Thus, it can be easily integrated
into existing software stacks [16, 2]. The source code is provided to
facilitate further evaluation and application.

##  VI Acknowledgments

This work was conducted during the author’s employment at Mech-Mind Robotics.
The views expressed in this paper are those of the authors themselves and are
not endorsed by the supporting agencies.

## References

  * [1] S. Boyd, S. P. Boyd, and L. Vandenberghe.  Convex optimization.  Cambridge university press, 2004.
  * [2] E. Coumans and Y. Bai.  Pybullet, a python module for physics simulation for games, robotics and machine learning.  2016\.
  * [3] D. Dobkin, J. Hershberger, D. Kirkpatrick, and S. Suri.  Computing the intersection-depth of polyhedra.  Algorithmica, 9(6):518–533, 1993.
  * [4] C. Ericson.  Real-time collision detection.  Crc Press, 2004.
  * [5] D. Fiser.  Libccd: Library for collision detection between two convex shapes, 2015\.
  * [6] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi.  A fast procedure for computing the distance between complex objects in three-dimensional space.  IEEE Journal on Robotics and Automation, 4:193–203, 1988.
  * [7] A. Gregory, A. Mascarenhas, S. Ehmann, M. Lin, and D. Manocha.  Six degree-of-freedom haptic display of polygonal models.  In Proceedings Visualization 2000., pages 139–146. IEEE, 2000.
  * [8] S. Jacobs.  Game programming gems 7.  2008\.
  * [9] D. Kappler, F. Meier, J. Issac, J. Mainprice, C. G. Cifuentes, M. Wüthrich, V. Berenz, S. Schaal, N. Ratliff, and J. Bohg.  Real-time perception meets reactive motion generation.  IEEE Robotics and Automation Letters, 3(3):1864–1871, 2018.
  * [10] Y. J. Kim, M. C. Lin, and D. Manocha.  Incremental penetration depth estimation between convex polytopes using dual-space expansion.  IEEE transactions on visualization and computer graphics, 10(2):152–163, 2004.
  * [11] V. Kumar and E. Todorov.  Mujoco haptix: A virtual reality system for hand manipulation.  In 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), pages 657–663. IEEE, 2015.
  * [12] K. Mamou and F. Ghorbel.  A simple and efficient approach for 3d mesh approximate convex decomposition.  In 2009 16th IEEE international conference on image processing (ICIP), pages 3501–3504. IEEE, 2009.
  * [13] W. A. McNeely, K. D. Puterbaugh, and J. J. Troy.  Six degree-of-freedom haptic rendering using voxel sampling.  In ACM SIGGRAPH 2005 Courses, pages 42–es. 2005.
  * [14] A. A. Melkman.  On-line construction of the convex hull of a simple polyline.  Information Processing Letters, 25(1):11–12, 1987.
  * [15] B. Mirtich.  Rigid body contact: Collision detection to force computation.  In Workshop on Contact Analysis and Simulation, IEEE Intl. Conference on Robotics and Automation, 1998.
  * [16] J. Pan, S. Chitta, and D. Manocha.  Fcl: A general purpose library for collision and proximity queries.  In IEEE International Conference on Robotics and Automation, pages 3859–3866. IEEE, 2012.
  * [17] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel.  Finding locally optimal, collision-free trajectories with sequential convex optimization.  Citeseer.
  * [18] E. Todorov.  Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in mujoco.  In 2014 IEEE International Conference on Robotics and Automation (ICRA), pages 6054–6061. IEEE, 2014.
  * [19] M. Toussaint, K. Allen, K. A. Smith, and J. B. Tenenbaum.  Differentiable physics and stable modes for tool-use and manipulation planning.
  * [20] J. C. Trinkle, J.-S. Pang, S. Sudarsky, and G. Lo.  On dynamic multi-rigid-body contact problems with coulomb friction.  ZAMM-Journal of Applied Mathematics and Mechanics/Zeitschrift für Angewandte Mathematik und Mechanik, 77(4):267–279, 1997.
  * [21] G. Van Den Bergen.  Proximity queries and penetration depth computation on 3d game objects.  In Game developers conference, volume 170, 2001.