# Safe Stabilizing Control for Polygonal Robots in Dynamic Elliptical
Environments

Kehan Long Khoa Tran Melvin Leok Nikolay Atanasov The authors are with the
Contextual Robotics Institute, University of California San Diego, La Jolla,
CA 92093, USA (e-mails: {k3long,k2tran,mleok,natanasov}@ucsd.edu).

###### Abstract

This paper addresses the challenge of safe navigation for rigid-body mobile
robots in dynamic environments. We introduce an analytic approach to compute
the distance between a polygon and an ellipse, and employ it to construct a
control barrier function (CBF) for safe control synthesis. Existing CBF design
methods for mobile robot obstacle avoidance usually assume point or circular
robots, preventing their applicability to more realistic robot body
geometries. Our work enables CBF designs that capture complex robot and
obstacle shapes. We demonstrate the effectiveness of our approach in
simulations highlighting real-time obstacle avoidance in constrained and
dynamic environments for both mobile robots and multi-joint robot arms.

##  I Introduction

Obstacle avoidance in static and dynamic environments is a central challenge
for safe mobile robot autonomy.

At the planning level, several motion planning algorithms have been developed
to provide a feasible path that ensures obstacle avoidance, including
prominent approaches like A∗ [1], RRT∗ [2], and their variants [3, 4]. These
algorithms typically assume that a low-level tracking controller can execute
the planned path. However, in dynamic environments where obstacles and
conditions change rapidly, reliance on such a controller can be limiting. A
significant contribution to the field was made by Khatib [5], who introduced
artificial potential fields to enable collision avoidance during not only the
motion planning stage but also the real-time control of a mobile robot. Later,
Rimon and Koditschek [6] developed navigation functions, a particular form of
artificial potential functions that guarantees simultaneous collision
avoidance and stabilization to a goal configuration. In recent years, research
has delved into the domain of trajectory generation and optimization, with
innovative algorithms proposed for quadrotor safe navigation [7, 8, 9]. In
parallel, the rise of learning-based approaches [10, 11, 12] has added a new
direction to the field, utilizing machine learning to facilitate both planning
and real-time obstacle avoidance. Despite their promise, these methods often
face challenges in dynamic environments and in providing safety guarantees.

In the field of safe control synthesis, integrating control Lyapunov functions
(CLFs) and control barrier functions (CBFs) into a quadratic program (QP) has
proven to be a reliable and efficient strategy for formulating safe
stabilizing controls across a wide array of robotic tasks [13, 14, 15]. While
CBF-based methodologies have been deployed for obstacle avoidance [16, 17, 18,
19, 20], such strategies typically simplify the robot as a point or circle and
assume static environments when constructing CBFs for control synthesis. Some
recent advances have also explored the use of time-varying CBFs to facilitate
safe control in dynamic environments [21, 22, 23]. However, this concept has
yet to be thoroughly investigated in the context of obstacle avoidance for
rigid-body robots. For the safe autonomy of robot arms, Koptev et al. [24]
introduced a neural network approach to approximate the signed distance
function of a robot arm and use it for safe reactive control in dynamic
environments. In [25], a CBF construction formula is proposed for a robot arm
with a static and circular obstacle. A configuration-aware control approach
for the robot arm was proposed in [26] by integrating geometric restrictions
with CBFs. Thirugnanam et al. [27] introduced a discrete CBF constraint
between polytopes and further incorporated the constraint in a model
predictive control to enable safe navigation. The authors also extended the
formulation for continuous-time systems in [28] but the CBF computation
between polytopes is numerical, requiring a duality-based formulation with
non-smooth CBFs.

#### Notations

The sets of non-negative real and natural numbers are denoted
$\mathbb{R}_{\geq 0}$ and $\mathbb{N}$. For $N\in\mathbb{N}$,
$[N]:=\\{1,2,\dots N\\}$. The orientation of a 2D body is denoted by
$0\leq\theta<2\pi$ for counter-clockwise rotation. We denote the corresponding
rotation matrix as
$\mathbf{R}(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta\\\
\sin\theta&\cos\theta\end{bmatrix}.$ The configuration of a 2D rigid-body is
described by position and orientation, and the space of the positions and
orientations in 2D is called the special Euclidean group, denoted as $SE(2)$.
Also, we use $\|\mathbf{x}\|$ to denote the $L_{2}$ norm for a vector
$\mathbf{x}$ and $\otimes$ to denote the Kronecker product. The gradient of a
differentiable function $V$ is denoted by $\nabla V$, and its Lie derivative
along a vector field $f$ by ${\cal L}_{f}V=\nabla V\cdot f$. A continuous
function $\alpha:[0,a)\rightarrow[0,\infty)$ is of class ${\cal K}$ if it is
strictly increasing and $\alpha(0)=0$. A continuous function
$\alpha:\mathbb{R}\rightarrow\mathbb{R}$ is of extended class ${\cal
K}_{\infty}$ if it is of class ${\cal K}$ and
$\lim_{r\rightarrow\infty}\alpha(r)=\infty$. Lastly, consider the body-fixed
frame of the ellipse ${\cal E}^{\prime}$. The signed distance function (SDF)
of the ellipse $\psi_{\cal E}:\mathbb{R}^{2}\to\mathbb{R}$ is defined as

$\psi_{\cal E}(\mathbf{p}^{\prime})=\left\\{\begin{array}[]{ll}d({\cal
E}^{\prime},\mathbf{p}^{\prime}),&\text{if }\mathbf{p}^{\prime}\in{\cal
E}^{c},\\\ -d({\cal E}^{\prime},\mathbf{p}^{\prime}),&\text{if
}\mathbf{p}^{\prime}\in{\cal E},\end{array}\right.$

where $d$ is the Euclidean distance. In addition, $\|\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})\|=1$ for all $\mathbf{p}^{\prime}$ except on the
boundary of the ellipse and its center of mass, the origin.

Contributions: (i) We present an analytic distance formula in $SE(2)$ for
elliptical and polygonal objects, enabling closed-form calculations for
distance and its gradient. (ii) We introduce a novel time-varying control
barrier function, specifically for rigid-body robots described by one or
multiple $SE(2)$ configurations. Its efficacy of ensuring safe autonomy is
demonstrated in ground robot navigation and multi-link robot arm problems.

##  II Problem Formulation

Consider a robot with dynamics governed by a non-linear control-affine system,

$\displaystyle\dot{\mathbf{x}}=f(\mathbf{x})+g(\mathbf{x})\mathbf{u},$ (1)

where $\mathbf{x}\in{\cal X}\subseteq\mathbb{R}^{n}$ is the robot state and
$\mathbf{u}\in\mathbb{R}^{m}$ is the control input. Assume that
$f:\mathbb{R}^{n}\mapsto\mathbb{R}^{n}$ and
$g:\mathbb{R}^{n}\mapsto\mathbb{R}^{n\times m}$ are continuously
differentiable functions. We assume the robot operates in a 2D workspace with
a state-dependent shape $S(\mathbf{x})\subset\mathbb{R}^{2}$.

We assume the $\mathbb{R}^{2}$ workspace is partitioned into a closed safe
(free) region $\mathcal{F}(t)$ and an open unsafe region $\mathcal{O}(t)$ such
that $\mathcal{F}(t)\cap\mathcal{O}(t)=\emptyset$ and
$\mathbb{R}^{2}=\mathcal{F}(t)\cup\mathcal{O}(t)$. We assume the unsafe set
$\mathcal{O}(t)$ is characterized by a collection of dynamical elliptical
obstacles with known rigid-body motions, denoted as $\\{{\cal
E}(\mathbf{q}_{i}(t),\mathbf{R}(\theta_{i}(t)),a_{i},b_{i})\\}_{i=1}^{N}$.
Here, $\mathbf{q}_{i}$ denotes the center of mass and $\mathbf{R}_{i}$ denotes
the rotation matrix of the ellipse. In its body-fixed frame, $a_{i}$ and
$b_{i}$ are the lengths of the semi-axes of the ellipse along the $x$-axis and
$y$-axis, respectively.

######  Problem.

Given a robot with shape $S(\mathbf{x})$ governed by dynamics (1) that can
perfectly determine its state, the objective is to stabilize the robot safely
within a goal region ${\cal G}\subset\mathbb{R}^{2}$ such that
$S(\mathbf{x}(t))\cap{\cal O}(t)=\emptyset$ for all $t\in\mathbb{R}_{\geq 0}$.

##  III Preliminaries

In this section, we review preliminaries on control Lyapunov and barrier
functions and discuss their use in synthesizing a safe stabilizing controller
for dynamics in (1).

###  III-A Control Lyapunov Function

The notion of a control Lyapunov function (CLF) was introduced in [29, 30] to
verify the stabilizability of control-affine systems (1). Specifically, a
(exponentially stabilizing) CLF $V:{\cal X}\mapsto\mathbb{R}$ is defined as
follows,

######  Definition III.1.

A function $V\in\mathbb{C}^{1}({\cal X},\mathbb{R})$ is a _control Lyapunov
function (CLF)_ on ${\cal X}$ for system (1) if
$V(\mathbf{x})>0,\forall\mathbf{x}\in{\cal
X}\setminus\\{\boldsymbol{0}\\},V(\boldsymbol{0})=0$, and it satisfies:

$\inf_{\mathbf{u}\in\mathbb{R}^{m}}\text{CLC}(\mathbf{x},\mathbf{u})\leq
0,\quad\forall\mathbf{x}\in{\cal X},$ (2)

where
$\text{CLC}(\mathbf{x},\mathbf{u}):=\mathcal{L}_{f}V(\mathbf{x})+\mathcal{L}_{g}V(\mathbf{x})\mathbf{u}+\alpha_{V}(V(\mathbf{x}))$
is the _control Lyapunov condition_ (CLC) defined for some class $K$ function
$\alpha_{V}$.

###  III-B Control Barrier Function

To facilitate safe control synthesis, we consider a time-varying set ${\cal
C}(t)$ defined as the super zero-level set of a continuously differentiable
function $h:{\cal X}\times\mathbb{R}\mapsto\mathbb{R}$:

${\cal C}(t):=\\{\mathbf{x}\in{\cal
X}\subseteq\mathbb{R}^{n}:h(\mathbf{x},t)\geq 0\\}.$ (3)

Safety of the system (1) can then be ensured by keeping the state $\mathbf{x}$
within the safe set ${\cal C}(t)$.

######  Definition III.2.

A function $h:\mathbb{R}^{n}\times\mathbb{R}_{\geq 0}\mapsto{\mathbb{R}}$ is a
valid time-varying _control barrier function (CBF)_ on
$\mathcal{X}\subseteq\mathbb{R}^{n}$ for (1) if there exists an extended class
$\mathcal{K}_{\infty}$ function $\alpha_{h}$ with:

$\sup_{\mathbf{u}\in\mathcal{U}}\text{CBC}(\mathbf{x},\mathbf{u},t)\geq
0,\quad\forall\;(\mathbf{x},t)\in{\cal X}\times\mathbb{R}_{\geq 0},$ (4)

where the _control barrier condition (CBC)_ is:

$\displaystyle\text{CBC}(\mathbf{x},\mathbf{u},t):=\dot{h}(\mathbf{x},t)+\alpha_{h}(h(\mathbf{x},t))$
(5)
$\displaystyle=\mathcal{L}_{f}h(\mathbf{x},t)+\mathcal{L}_{g}h(\mathbf{x},t)\mathbf{u}+\frac{\partial
h(\mathbf{x},t)}{\partial t}+\alpha_{h}(h(\mathbf{x},t)).$

Definition III.2 allows us to consider the following set of control values
that render the safe set ${\cal C}$ forward invariant,

$K_{\text{CBF}}(\mathbf{x},t):=\left\\{\mathbf{u}\in{\cal
U}:\text{CBC}(\mathbf{x},\mathbf{u},t)\geq 0\right\\}.$ (6)

Suppose we are given a baseline feedback controller
$\mathbf{u}=\mathbf{k}(\mathbf{x})$ for the control-affine systems (1), and we
aim to ensure the safety and stability of the system. By observing that both
the CLC and CBC constraints are affine in the control input $\mathbf{u}$, a
quadratic program (QP) can be formulated for online synthesis of a safe
stabilizing controller for (1):

$\displaystyle\mathbf{u}(\mathbf{x})=$
$\displaystyle\operatorname*{arg\,min}_{\mathbf{u}\in\mathbb{R}^{m},\delta\in\mathbb{R}}\|\mathbf{u}-\mathbf{k}(\mathbf{x})\|^{2}+\lambda\delta^{2},$
(7) $\displaystyle\mathrm{s.t.}$
$\displaystyle\text{CLC}(\mathbf{x},\mathbf{u})\leq\delta,\text{CBC}(\mathbf{x},\mathbf{u},t)\geq
0,$

where $\delta\geq 0$ denotes a slack variable that relaxes the CLF constraints
to ensure the feasibility of the QP, controlled by the scaling factor
$\lambda>0$.

##  IV Analytic distance between ellipse and polygon

In this section, we derive an analytic formula for computing the distance
between a polygon and an ellipse. From this distance function, we also compute
the associated partial derivatives, which enable the formulation of CBFs to
ensure safe autonomy.

We consider the mobile robot’s body $S(\mathbf{x})$ to be described as a
polygon, denoted by ${\cal
P}(\tilde{\mathbf{q}},\tilde{\mathbf{R}}(\tilde{\theta}),\\{\tilde{\mathbf{p}}_{i}\\}_{i=0}^{M-1})$.
Here, $\tilde{\mathbf{q}}$ denotes the center of mass and $\tilde{\mathbf{R}}$
denotes the orientation in the inertial frame. In its fixed-body frame,
$\\{\tilde{\mathbf{p}}_{i}\\}$ denotes the vertices of the polygonal robot
with line segments
$\tilde{\mathbf{d}}_{i}=\tilde{\mathbf{p}}_{[i+1]_{M}}-\tilde{\mathbf{p}}_{i}$
for $i=0,1,\ldots,M-1$ where $[\cdot]_{M}$ is the $M$-modulus.

For convenience, denote ${\cal E}$ and ${\cal P}$ as the bodies in the
inertial frame, and we assume their intersection is empty. Now, denote ${\cal
E}^{\prime}$ and ${\cal P}^{\prime}$ as the respective bodies in the body-
fixed frame of the elliptical obstacle. As a result,

$d({\cal E},{\cal P})=d({\cal E}^{\prime},{\cal P}^{\prime})$ (8)

by isometric transformation. Furthemore, let $\tilde{\mathbf{p}}_{i}$ be a
vertex in the robot’s frame. Then in the inertial frame, it becomes
$\mathbf{p}_{i}=\tilde{\mathbf{q}}+\tilde{\mathbf{R}}\tilde{\mathbf{p}}_{i}$.
In the obstacle’s frame, it is

$\displaystyle\mathbf{p}_{i}^{\prime}=\mathbf{R}^{\top}(\mathbf{p}_{i}-\mathbf{q})=\mathbf{R}^{\top}\tilde{\mathbf{R}}\tilde{\mathbf{p}}_{i}+\mathbf{R}^{\top}(\tilde{\mathbf{q}}-\mathbf{q}),$
(9)

In short, $\\{\tilde{\mathbf{p}}_{i}\\}$ are vertices in the robot’s frame,
$\\{\mathbf{p}_{i}\\}$ are vertices in the inertial frame, and
$\\{\mathbf{p}_{i}^{\prime}\\}$ are vertices in the obstacle’s frame.

The distance function is

$d({\cal E}^{\prime},{\cal P}^{\prime}):=\min_{0\leq i<M}d({\cal
E}^{\prime},\mathbf{d}_{i}^{\prime}),$ (10)

which is the distance between the ellipse ${\cal E}^{\prime}$ and each line
segment $\mathbf{d}_{i}^{\prime}$. We write each segment as

$l_{i}^{\prime}(\tau)=(1-\tau)\mathbf{p}_{i}^{\prime}+\tau\mathbf{p}_{[i+1]_{M}}^{\prime},$
(11)

for $\tau\in[0,1]$. This further simplifies the function to

$d({\cal E}^{\prime},\mathbf{d}_{i}^{\prime})=\min_{\tau\in[0,1]}d({\cal
E}^{\prime},l_{i}^{\prime}(\tau)).$ (12)

Now, there are essentially two group of computations for the distance in (12):
one is the distances between the ellipse ${\cal E}^{\prime}$ and the endpoints
of $\mathbf{d}_{i}^{\prime}$; the other is the distance between the ellipse
${\cal E}^{\prime}$ and the infinite line $l_{i}^{\prime}(\tau)$ for arbitrary
$\tau$ with the caveat that the minimizing argument occurs at
$\tau^{*}\in(0,1)$. The two computations are detailed in the procedures which
follow our next proposition.

######  Proposition IV.1.

Let ${\cal E}^{\prime}$ be an ellipse and $l_{i}^{\prime}$ be a line segment
in the frame of the ellipse. Denote $\tau^{*}$ as the argument of the minimum
in (12). Then, the distance

$d({\cal
E}^{\prime},\mathbf{d}_{i}^{\prime})=\left\\{\begin{array}[]{ll}\|\mathbf{p}_{i}^{\prime}-\underline{\mathbf{p}_{i}^{\prime}}\|,&\text{if
}\tau^{*}=0,\\\
\|\mathbf{p}_{[i+1]_{M}}^{\prime}-\underline{\mathbf{p}_{[i+1]_{M}}^{\prime}}\|,&\text{if
}\tau^{*}=1,\\\
\|l_{i}^{\prime}(\tau^{*})-\underline{l_{i}^{\prime}(\tau^{*})}\|,&\text{if
}\tau^{*}\in(0,1).\end{array}\right.$ (13)

The points $\underline{\mathbf{p}_{i}^{\prime}}$ and
$\underline{\mathbf{p}_{[i+1]_{M}}^{\prime}}$ on the ellipse are determined
using Procedure 1, and both $l_{i}^{\prime}(\tau^{*})$ and
$\underline{l_{i}^{\prime}(\tau^{*})}$ (on the ellipse) are determined using
Procedure 2.

######  Procedure 1.

Let $\mathbf{p}^{\prime}=(p_{x}^{\prime},p_{y}^{\prime})$ be one of the
endpoints for the line segment $\mathbf{d}_{i}^{\prime}$. Recall that the
ellipse is defined by its semi-axes along $x$-axis and $y$-axis, denoted by
$a$ and $b$, respectively. Then, the points on the ellipse are parameterized
by

$x(t)=a\cos(t),\quad y(t)=b\sin(t),$ (14)

for $0\leq t\leq 2\pi$. The goal is to determine the point $(x(t),y(t))$ on
the ellipse that is closest to the point $\mathbf{p}^{\prime}$, so it is a
minimization problem of the squared Euclidean distance:

$d^{2}(t)=(p_{x}^{\prime}-a\cos(t))^{2}+(p_{y}^{\prime}-b\sin(t))^{2}.$ (15)

To find the minimum distance, we determine the critical point(s) by solving
for $0=\frac{d}{dt}d^{2}(t)$, which simplified to

$0=(b^{2}-a^{2})\cos t\sin t+ap_{x}^{\prime}\sin t-bp_{y}^{\prime}\cos t.$
(16)

Using single-variable optimization, we substitute

$\cos t=\lambda,\quad\sin t=\sqrt{1-\lambda},$ (17)

and this yields
$bp_{y}^{\prime}\lambda=\sqrt{1-\lambda^{2}}((b^{2}-a^{2})\lambda+ap_{x}^{\prime})$,
which is a quartic equation in $\lambda$. Furthermore, a monic quartic can be
derived, which gives the following simplified coefficients:

$0=\lambda^{4}+2m\lambda^{3}+(m^{2}+n^{2}-1)\lambda^{2}-2m\lambda-m^{2},$ (18)

where

$m=p_{x}^{\prime}\frac{a}{b^{2}-a^{2}},\quad
n=p_{y}^{\prime}\frac{b}{b^{2}-a^{2}}.$ (19)

From this point, the real root(s) of the equation can be solved analytically
following Cardano’s and Ferrari’s solution for the quartic equations [31]. Let
$\underline{t}$ be the solution so that
$\underline{\mathbf{p}^{\prime}}=(x(\underline{t}),y(\underline{t}))$ is a
point on the ellipse and is closest to $\mathbf{p}^{\prime}$. Hence,

$d({\cal
E}^{\prime},\mathbf{d}_{i}^{\prime})=\|\mathbf{p}^{\prime}-\underline{\mathbf{p}^{\prime}}\|$
(20)

where $\mathbf{p}^{\prime}$ is either $\mathbf{p}_{i}^{\prime}$ or
$\mathbf{p}_{[i+1]_{M}}^{\prime}$. This concludes Procedure 1.

######  Procedure 2.

We compute the distance between the ellipse ${\cal E}^{\prime}$ and the
infinite line $l_{i}^{\prime}(\tau)$ whose minimizing point occurs at
$\tau^{*}\in(0,1)$.

First define the unit normal of the infinite line as

$\hat{\mathbf{n}}_{i}^{\prime}=\frac{1}{\|\mathbf{d}_{i}^{\prime}\|}(-d^{\prime}_{i,y},d^{\prime}_{i,x}).$
(21)

Denote $\underline{l_{i}^{\prime}(\tau^{*})}$ as the point on the ellipse that
is closest to the $l_{i}^{\prime}(\tau^{*})$. In fact, this point
$\underline{l_{i}^{\prime}(\tau^{*})}$ must have a tangent line at the ellipse
which is parallel to $l_{i}^{\prime}$; this also means the normal at
$\underline{l_{i}^{\prime}(\tau^{*})}$ is $\pm\hat{\mathbf{n}}_{i}^{\prime}$,
so we can compute this point on the ellipse up to a sign:

$\underline{l_{i}^{\prime}(\tau^{*})}=\pm\frac{I_{\epsilon}^{2}\hat{\mathbf{n}}_{i}^{\prime}}{\|I_{\epsilon}\hat{\mathbf{n}}_{i}^{\prime}\|},$
(22)

where $I_{\epsilon}=\text{diag}(a,b)$. The correct sign is chosen when we are
looking at the sign of the constant $C$ in the line equation $Ax+By+C=0$ of
$l_{i}^{\prime}$. In particular,

$C=-\hat{\mathbf{n}}_{i}^{\prime\top}\mathbf{p}_{i}^{\prime}.$ (23)

If $C>0$, then
$\underline{l_{i}^{\prime}(\tau^{*})}=-\frac{I_{\epsilon}^{2}\hat{\mathbf{n}}_{i}^{\prime}}{\|I_{\epsilon}\hat{\mathbf{n}}_{i}^{\prime}\|}$;
otherwise, if $C<0$, then
$\underline{l_{i}^{\prime}(\tau^{*})}=\frac{I_{\epsilon}^{2}\hat{\mathbf{n}}_{i}^{\prime}}{\|I_{\epsilon}\hat{\mathbf{n}}_{i}^{\prime}\|}$.

Finally, we can determine $l_{i}^{\prime}(\tau^{*})$ on the line segment
$\mathbf{d}_{i}^{\prime}$ using projection:

$l_{i}^{\prime}(\tau^{*})=\mathbf{p}_{i}^{\prime}+\text{proj}_{\mathbf{d}_{i}^{\prime}}(\underline{l_{i}^{\prime}(\tau^{*})}-\mathbf{p}_{i}^{\prime}).$
(24)

Here, we are done with Procedure 2.

We turn our attention to compute the partial derivatives of $d({\cal
E}^{\prime},{\cal P}^{\prime})$ with respect to either
$(\mathbf{q},\mathbf{R})$, the configuration of our obstacle, or
$(\tilde{\mathbf{q}},\tilde{\mathbf{R}})$, the configuration of the polygonal
robot.

Now, in general, both procedures above compute the distance using the
Euclidean norm between two unique points: one point $\mathbf{p}^{\prime}$ on a
line segment of the robot, and the other $\underline{\mathbf{p}^{\prime}}$ on
the ellipse. This is, in fact, equivalent to the SDF of the ellipse evaluated
at $\mathbf{p}^{\prime}$ by the uniqueness of these two points. Therefore, let
$\mathbf{p}^{\prime}=l_{i}(\tau^{*})$ for some $0\leq i<M$, then

$d({\cal E}^{\prime},{\cal P}^{\prime})=\psi_{\cal
E}(\mathbf{p}^{\prime})=\psi_{\cal E}(l_{i}(\tau^{*})).$ (25)

Then, its gradient with respect to $\mathbf{p}^{\prime}$ is

$\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})=\frac{\mathbf{p}^{\prime}-\underline{\mathbf{p}^{\prime}}}{\|\mathbf{p}^{\prime}-\underline{\mathbf{p}^{\prime}}\|}.$
(26)

However, notice that $\mathbf{p}^{\prime}$ is a point transformed from the
polygonal robot’s frame using (9), which depends on both the configurations of
the elliptical obstacle and the polygonal robot. Hence the partial derivatives
can be computed using the chain rule:

######  Proposition IV.2.

Let ${\cal E}^{\prime}$ and ${\cal P}^{\prime}$ be the bodies of the
elliptical obstacle and robot, respectively, in the obstacle’s frame. Let
$\mathbf{p}^{\prime}$ and $\underline{\mathbf{p}^{\prime}}$ be determined from
Proposition IV.1, then

$\displaystyle\frac{\partial d}{\partial\mathbf{q}}$
$\displaystyle=\left(\frac{\partial d}{\partial q_{x}},\frac{\partial
d}{\partial q_{y}}\right)=-\mathbf{R}\nabla\psi_{\cal
E}(\mathbf{p}^{\prime}),$ (27) $\displaystyle\frac{\partial
d}{\partial\mathbf{R}}$ $\displaystyle=\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})\otimes(\tilde{\mathbf{R}}\tilde{\mathbf{p}}+(\tilde{\mathbf{q}}-\mathbf{q})),$
(28) $\displaystyle\frac{\partial d}{\partial\tilde{\mathbf{q}}}$
$\displaystyle=\left(\frac{\partial d}{\partial\tilde{q}_{x}},\frac{\partial
d}{\partial\tilde{q}_{y}}\right)=\mathbf{R}\nabla\psi_{\cal
E}(\mathbf{p}^{\prime}),$ (29) $\displaystyle\frac{\partial
d}{\partial\tilde{\mathbf{R}}}$ $\displaystyle=\mathbf{R}(\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})\otimes\tilde{\mathbf{p}}).$ (30)

Furthermore, Eqs. (28) and (30) are derivatives with respect to the rotation
matrices; one may compute the derivatives with respect to the rotation angle
as

$\displaystyle\begin{split}\frac{\partial d}{\partial\theta}&=\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})^{\top}\left[\frac{\partial\mathbf{R}}{\partial\theta}^{\top}(\tilde{\mathbf{R}}\tilde{\mathbf{p}}+(\tilde{\mathbf{q}}-\mathbf{q}))\right]\\\
&=\text{tr}\left[\frac{\partial
d}{\partial\mathbf{R}}\frac{\partial\mathbf{R}}{\partial\theta}\right],\end{split}$
(31) $\displaystyle\begin{split}\frac{\partial
d}{\partial\tilde{\theta}}&=\nabla\psi_{\cal
E}(\mathbf{p}^{\prime})^{\top}\left[\mathbf{R}^{\top}\frac{\partial\tilde{\mathbf{R}}}{\partial\tilde{\theta}}\tilde{\mathbf{p}}\right]=\text{tr}\left[\frac{\partial
d}{\partial\tilde{\mathbf{R}}}\frac{\partial\tilde{\mathbf{R}}}{\partial\tilde{\theta}}^{\top}\right].\end{split}$
(32)

Following both propositions above, we can finally implement CBF using the
distance function

$\Phi(\mathbf{q},\mathbf{R},\tilde{\mathbf{q}},\tilde{\mathbf{R}})=d({\cal
E},{\cal P})=d({\cal E}^{\prime},{\cal P}^{\prime})$ (33)

for the elliptical obstacle ${\cal E}(\mathbf{q},\mathbf{R},a,b)$ and
polygonal robot ${\cal
P}(\tilde{\mathbf{q}},\tilde{\mathbf{R}},\\{\tilde{\mathbf{p}}_{i}\\})$.
Furthermore, $\Phi$ is differentiable with respect to both
$(\mathbf{q},\mathbf{R})$ and $(\tilde{\mathbf{q}},\tilde{\mathbf{R}})$.

##  V Polygon-Shaped Robot Safe Navigation in Dynamic Ellipse Environments

In this section, using the distance formula in (33) and assuming known motion
of the ellipse obstacles, we derive time-varying control barrier functions to
ensure safety for polygon-shaped robots operating in dynamic elliptical
environments.

###  V-A Time-Varying Control Barrier Function Constraints

We assume that there are a total of $N$ elliptical obstacles in the
environment, each having a rigid-body motion with linear velocity $v_{i}$ and
angular velocity $\omega_{i}$ around its center of mass. We define a time-
varying CBF

$h(\mathbf{x},t):=\min_{i=1}^{N}\Phi(\mathbf{q}_{i}(t),\mathbf{R}_{i}(t),\tilde{\mathbf{q}},\tilde{\mathbf{R}}),$
(34)

where $\Phi$ is the collision function and $\mathbf{q}_{i}(t)$ and
$\mathbf{R}_{i}(t)$ denotes the position and orientation of the $i$-th ellipse
at time $t$, respectively.

Based on the TV-CBF definition, we construct the safety constraint as follows.
We first identify the index $k$ corresponding to the minimum collision
function:

$k=\arg\min_{i=1}^{N}\Phi(\mathbf{q}_{i}(t),\mathbf{R}_{i}(t),\tilde{\mathbf{q}},\tilde{\mathbf{R}}),$
(35)

and we can compute the time derivative of the CBF as:

$\frac{\partial h(\mathbf{x},t)}{\partial
t}=\frac{\partial\Phi(\mathbf{q}_{k},\mathbf{R}_{k})}{\partial\mathbf{q}_{k}}\frac{\partial\mathbf{q}_{k}}{\partial
t}+\frac{\partial\Phi(\mathbf{q}_{k},\mathbf{R}_{k})}{\partial\mathbf{R}_{k}}\frac{\partial\mathbf{R}_{k}}{\partial
t}.$ (36)

Figure 1: Comparative analysis of the $SE(2)$ and $\mathbb{R}^{2}$ signed
distance functions for elliptical obstacles. The cyan triangle represents the
rigid-body robot, with its orientation varying across the sequence. The
importance of considering robot orientation in distance computations becomes
evident: while the $SE(2)$ function accounts for this orientation, the
$\mathbb{R}^{2}$ approximation treats the robot as an encapsulating circle
with radius $1$. Level sets at distances $0.2$ and $2$ are depicted for both
functions.

Now, by utilizing the known motion of the $k$-th ellipse with linear and
angular velocity $v_{k}$ and $\omega_{k}$, we can express the CBC condition
as:

$\displaystyle\textit{CBC}(\mathbf{x},\mathbf{u},t)$
$\displaystyle:=\left[\frac{\partial\Phi(\mathbf{q}_{k},\mathbf{R}_{k},\mathbf{x})}{\partial\mathbf{x}}\right]^{\top}F(\mathbf{x})\underline{\mathbf{u}}+\frac{\partial\Phi(\mathbf{q}_{k},\mathbf{R}_{k})}{\partial\mathbf{q}_{k}}v_{k}$
$\displaystyle+\frac{\partial\Phi(\mathbf{q}_{k},\mathbf{R}_{k})}{\partial\mathbf{R}_{k}}\frac{\partial\mathbf{R}_{k}}{\partial\theta_{k}}\omega_{k}+\alpha_{h}(\Phi(\mathbf{q}_{k},\mathbf{R}_{k},\mathbf{x}))\geq
0.$

###  V-B Ground-Robot Navigation

(a) Initial Pose  (b) Time t = 1.66 sec  (c) Time t = 4.12 sec  (d) Final Pose
(e) Circular Robot

Figure 2: Safe navigation in a dynamical elliptical environment. (a) shows the
initial pose of the triangular robot and the environment. (b) shows the
triangular robot passing through the narrow space between two moving ellipses.
(c) shows the robot adjusts its pose to avoid the moving obstacle. (d) shows
the final pose of the robot that reaches the goal region and the current
environment. In (e), we plot the trajectory of navigating a circular robot in
the same environment.

Suppose the robot has a polygonal shape with $\\{\tilde{\mathbf{p}}_{i}\\}$
denoting the vertices, and the robot motion is governed by unicycle
kinematics,

$\begin{bmatrix}\dot{x}\\\ \dot{y}\\\
\dot{\theta}\end{bmatrix}=\begin{bmatrix}\cos(\theta)&0\\\ \sin(\theta)&0\\\
0&1\end{bmatrix}\begin{bmatrix}v\\\ \omega\end{bmatrix},$ (37)

where $v$, $\omega$ represent the robot linear and angular velocity,
respectively. The state and input are
$\mathbf{x}:=[x,y,\theta]^{\top}\in\mathbb{R}^{2}\times[-\pi,\pi)$,
$\mathbf{u}:=[v,\omega]^{\top}\in\mathbb{R}^{2}$. The CLF for the unicycle
model is defined as a quadratic form
$V(\mathbf{x})=(\mathbf{x}-\mathbf{x}^{*})^{\top}\mathbf{Q}(\mathbf{x}-\mathbf{x}^{*})$,
where $\mathbf{x}^{*}$ denotes the desired equilibrium and $\mathbf{Q}$ is a
positive-definite matrix [32]. We then define the goal region ${\cal G}$ as a
disk centered at the 2D position of the desired state $\mathbf{x}^{*}$, with a
radius $r$.

By writing the robot’s position as $\tilde{\mathbf{q}}=[x,y]^{\top}$ and its
orientation via the rotation matrix $\tilde{\mathbf{R}}(\theta)$, we write the
shape $S(\mathbf{x})$ of the robot in terms of its state:

$S(\mathbf{x}):=\text{conv}\\{\tilde{\mathbf{q}}+\tilde{\mathbf{R}}(\theta)\tilde{\mathbf{p}_{i}}\\}$
(38)

where $\tilde{\mathbf{p}_{i}}$ denotes the vertices of the polygon and
$\text{conv}\\{\cdot\\}$ denotes the convex hull of points. With this
definition, we can derive the CBF for the polygon-shaped unicycle model, as in
(34).

###  V-C K-joint Robot Arm Safe Stabilizing Control

(a) Initial Pose  (b) Time t = 4.12 sec  (c) Time t = 4.92 sec  (d) Time t =
6.28 sec  (e) Final Pose

Figure 3: Safe stabilization of a 3-joint robot arm. The green circle denotes
the goal region, and the gray box denotes the base of the arm. The arm is
shown in blue and the trajectory of its end-effector is shown in red. The
trajectories of the moving elliptical obstacles are shown in purple.

In this section, we discuss methods for controlling a 2D K-joint robot arm in
a dynamical ellipse environment by utilizing our proposed CBF construction
approach. For such robots, the links are intrinsically interconnected due to
kinematic chaining. This means that controlling any one link will influence
the pose of all subsequent links.

The dynamics of the robot arm are captured by:

$\dot{\boldsymbol{\theta}}=\boldsymbol{\omega},$ (39)

where
$\boldsymbol{\theta}=[\tilde{\theta}_{1},\tilde{\theta}_{2},\ldots,\tilde{\theta}_{K}]^{\top}$
and $\boldsymbol{\omega}=[\omega_{1},\omega_{2},\ldots,\omega_{K}]^{\top}$.

For the robot arm, each link has an associated 2D shape, denoted as
$S_{i}(\boldsymbol{\theta})$, which depends on the state of the arm. The
overall shape of the robot arm, is given by the union of these shapes
$S(\boldsymbol{\theta})=\bigcup_{i=1}^{K}S_{i}(\boldsymbol{\theta})$. For
simplicity, we assume each $S_{i}$ is a line segment.

For each link $i$, its state in $SE(2)$ consists of a position
$\tilde{\mathbf{q}}_{i}=[x_{i},y_{i}]^{\top}$:

$\displaystyle x_{i}$
$\displaystyle=x_{i-1}+L_{i}\cos\left(\sum_{j=1}^{i}\tilde{\theta}_{j}\right),$
(40) $\displaystyle y_{i}$
$\displaystyle=y_{i-1}+L_{i}\sin\left(\sum_{j=1}^{i}\tilde{\theta}_{j}\right),$
(41)

and a rotation matrix $\tilde{\mathbf{R}}_{i}$ corresponding to
$\underline{\theta_{i}}:=\sum_{j=1}^{i}\tilde{\theta}_{j}$. For simplicity, we
suppose $x_{1}=0$ and $y_{1}=0$, and $L_{i}$ represents the length of the
$i$-th link. The robot state can also be represented as multiple $SE(2)$
configurations corresponding to each link, from
$(\tilde{\mathbf{q}}_{1},\tilde{\mathbf{R}}_{1})$ to
$(\tilde{\mathbf{q}}_{K},\tilde{\mathbf{R}}_{K})$. Additionally, we denote
$\tilde{\mathbf{q}}_{K+1}$ as the end effector.

We define the CLF for the K-joint robot arm as
$V(\boldsymbol{\theta})=(\boldsymbol{\theta}-\boldsymbol{\theta}^{*})^{\top}\mathbf{Q}(\boldsymbol{\theta}-\boldsymbol{\theta}^{*}),$
where $\mathbf{Q}$ is a positive-definite matrix, and
$\boldsymbol{\theta}^{*}$ is the desired joint states. The goal region ${\cal
G}$ is specified as a disk centered at the position of the end effector
corresponding to state $\boldsymbol{\theta}^{*}$, with a defined radius $r$.

For safety assurance, the CBF is constructed using the distance between the
robot arm and nearby elliptical obstacles:

$h(\boldsymbol{\theta})=\min_{i=1,\ldots,N,\;j=1,\ldots,M}\Phi(\mathbf{q}_{i},\mathbf{R}_{i},\tilde{\mathbf{q}}_{j},\tilde{\mathbf{R}}_{j}).$
(42)

##  VI Evaluation

In this section, we show the efficacy of our proposed CBF construction
techniques using simulation examples, focusing on ground-robot navigation and
2-D robot arm control.

Fig. 1 contrasts the $SE(2)$ distance function with the $\mathbb{R}^{2}$
counterpart by visualizing their level sets. Our proposed $SE(2)$ approach
incorporates the orientation of the rigid-body robot, yielding notably
improved results, particularly when the robot is close to obstacles.

To highlight the significance of accurate robot shape representation, we draw
a comparison with a baseline circular robot CBF formulation. In Fig. 2, we
compare safe navigation using our proposed $SE(2)$ CBF approach with a regular
$\mathbb{R}^{2}$ CBF approach. For both methods, we set
${\mathbf{k}}(\mathbf{x})=[v_{\max},0]^{\top}$ where $v_{\max}=3.0$ is the
maximum linear velocity. The remaining parameters were $\lambda=100$,
$\alpha_{V}(V(\mathbf{x}))=2V(\mathbf{x})$, and
$\alpha_{h}(h(\mathbf{x},t))=3h(\mathbf{x},t)$.

We demonstrate safe navigation to a goal state. In Fig. 2(a), the triangular
robot starts the navigation with position centered at $(0,0)$ and orientation
$\theta=\pi/4$. In Fig. 2(b), the robot adeptly navigates the narrow passage
between two dynamic obstacles. In Fig. 2(d), we see that the robot is able to
reach the goal region without collision. In Fig. 2(e), when the robot is
conservatively modeled as a circle navigating the identical environment, it is
evident that the robot has to opt for a more circuitous route to circumvent
obstacles. This is due to its inability to traverse certain constricted
spaces, as illustrated in Fig. 2(b). These outcomes underscore the superior
performance of our $SE(2)$ CBF methodology. Another advantage of the $SE(2)$
formulation lies in its assurance of a uniformly relative degree of $1$ for
the constructed CBF, obviating the need to model a point off the wheel axis
[33].

In the following set of experiments, we consider safe stabilization of a
3-joint robot arm in a dynamical elliptical environment. We set
${\mathbf{k}}(\mathbf{x})=[0,0,0]^{\top}$ and restrict the joint control
bounds with $|\omega_{i}|\leq 3$. In Fig. 3, the robot arm is able to elude
the mobile ellipses by nimbly adjusting its pose. In Fig. 4, we show the
control inputs of each joint over time. We see that when the robot arm is
close to the obstacles, it is able to take large control inputs in adjusting
its pose. In Fig. 5, we show the CLF and CBF values over time. A consistently
positive CBF value throughout the trajectory signifies safety assurance, while
the decreasing CLF values indicates the convergence to the desired state.
Moreover, the CLF value may increase when the arm is close to obstacles (i.e.
CBF value is low), this comes from the relaxation of the CLF-CBF QP to ensure
the feasibility of the program.

Figure 4: Control input of the 3-joint robot arm.  Figure 5: Lyapunov function
and barrier function values over time.

##  VII Conclusion

We present an analytic distance formula between elliptical and polygonal
objects. Leveraging this formula, we construct a time-varying control barrier
function that ensures the safe autonomy of a polygon-shaped robot operating in
dynamical elliptical environments. The efficacy of the proposed approach is
demonstrated in rigid-body navigation and multi-link robot arm problems.
Future work will consider extending the formulation to 3-D environments (e.g.
drone navigation, robot arm manipulation) and estimating the geometry and
dynamics of the environment with on-board sensing.

## References

  * [1] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.
  * [2] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The international journal of robotics research, vol. 30, no. 7, pp. 846–894, 2011.
  * [3] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in 2014 IEEE/RSJ international conference on intelligent robots and systems, pp. 2997–3004, IEEE, 2014.
  * [4] J. Wang, W. Chi, C. Li, C. Wang, and M. Q.-H. Meng, “Neural rrt*: Learning-based optimal path planning,” IEEE Transactions on Automation Science and Engineering, vol. 17, no. 4, pp. 1748–1758, 2020.
  * [5] O. Khatib, “Real-Time Obstacle Avoidance for Manipulators and Mobile Robots,” The International Journal of Robotics Research, vol. 5, no. 1, pp. 90–98, 1986.
  * [6] E. Rimon and D. E. Koditschek, “Exact robot navigation using artificial potential functions,” IEEE Transactions on Robotics and Automation, vol. 8, no. 5, pp. 501–518, 1992.
  * [7] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in 2011 IEEE International Conference on Robotics and Automation, pp. 2520–2525, 2011.
  * [8] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.
  * [9] J. Tordesillas, B. T. Lopez, and J. P. How, “Faster: Fast and safe trajectory planner for flights in unknown environments,” in 2019 IEEE/RSJ international conference on intelligent robots and systems (IROS), pp. 1934–1940, IEEE, 2019.
  * [10] J. Michels, A. Saxena, and A. Y. Ng, “High speed obstacle avoidance using monocular vision and reinforcement learning,” in Proceedings of the 22nd international conference on Machine learning, pp. 593–600, 2005.
  * [11] M. Pfeiffer, S. Shukla, M. Turchetta, C. Cadena, A. Krause, R. Siegwart, and J. Nieto, “Reinforced imitation: Sample efficient deep reinforcement learning for mapless navigation by leveraging prior demonstrations,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 4423–4430, 2018.
  * [12] A. Loquercio, E. Kaufmann, R. Ranftl, M. Müller, V. Koltun, and D. Scaramuzza, “Learning high-speed flight in the wild,” Science Robotics, vol. 6, no. 59, p. eabg5810, 2021.
  * [13] P. Glotfelter, J. Cortés, and M. Egerstedt, “Nonsmooth barrier functions with applications to multi-robot systems,” IEEE Control Systems Letters, vol. 1, no. 2, pp. 310–315, 2017.
  * [14] R. Grandia, A. J. Taylor, A. D. Ames, and M. Hutter, “Multi-layered safety for legged robots via control barrier functions and model predictive control,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 8352–8358, 2021.
  * [15] L. Wang, A. D. Ames, and M. Egerstedt, “Safe certificate-based maneuvers for teams of quadrotors using differential flatness,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 3293–3298, IEEE, 2017.
  * [16] M. Srinivasan, A. Dabholkar, S. Coogan, and P. Vela, “Synthesis of control barrier functions using a supervised machine learning approach,” IEEE/RSJ IROS, pp. 7139–7145, 2020.
  * [17] K. Long, C. Qian, J. Cortés, and N. Atanasov, “Learning barrier functions with memory for robust safe navigation,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 4931–4938, 2021.
  * [18] H. Almubarak, K. Stachowicz, N. Sadegh, and E. A. Theodorou, “Safety embedded differential dynamic programming using discrete barrier states,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 2755–2762, 2022.
  * [19] C. Dawson, B. Lowenkamp, D. Goff, and C. Fan, “Learning safe, generalizable perception-based hybrid control with certificates,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1904–1911, 2022.
  * [20] H. Abdi, G. Raja, and R. Ghabcheloo, “Safe control using vision-based control barrier function (v-cbf),” in 2023 IEEE International Conference on Robotics and Automation (ICRA), pp. 782–788, IEEE, 2023.
  * [21] S. He, J. Zeng, B. Zhang, and K. Sreenath, “Rule-based safety-critical control design using control barrier functions with application to autonomous lane change,” in 2021 American Control Conference (ACC), pp. 178–185, IEEE, 2021.
  * [22] T. G. Molnar, A. K. Kiss, A. D. Ames, and G. Orosz, “Safety-critical control with input delay in dynamic environment,” IEEE transactions on control systems technology, 2022.
  * [23] V. Hamdipoor, N. Meskin, and C. G. Cassandras, “Safe control synthesis using environmentally robust control barrier functions,” European Journal of Control, p. 100840, 2023.
  * [24] M. Koptev, N. Figueroa, and A. Billard, “Neural joint space implicit signed distance functions for reactive robot manipulator control,” IEEE Robotics and Automation Letters, vol. 8, no. 2, pp. 480–487, 2023.
  * [25] R. Hamatani and H. Nakamura, “Collision avoidance control of robot arm considering the shape of the target system,” in 2020 59th Annual Conference of the Society of Instrument and Control Engineers of Japan (SICE), pp. 1311–1316, 2020.
  * [26] F. Ding, H. Wang, J. He, Y. Ren, and Y. Zheng, “Configuration-aware safe control for mobile robotic arm with control barrier functions,” 2022.
  * [27] A. Thirugnanam, J. Zeng, and K. Sreenath, “Safety-critical control and planning for obstacle avoidance between polytopes with control barrier functions,” in 2022 International Conference on Robotics and Automation (ICRA), pp. 286–292, 2022.
  * [28] A. Thirugnanam, J. Zeng, and K. Sreenath, “Duality-based convex optimization for real-time obstacle avoidance between polytopes with control barrier functions,” in 2022 American Control Conference (ACC), pp. 2239–2246, 2022.
  * [29] Z. Artstein, “Stabilization with relaxed controls,” Nonlinear Analysis-theory Methods & Applications, vol. 7, pp. 1163–1173, 1983.
  * [30] E. D. Sontag, “A ‘universal’ construction of Artstein’s theorem on nonlinear stabilization,” Systems & Control Letters, vol. 13, no. 2, pp. 117–123, 1989.
  * [31] H. Cardano, Artis magnae, sive de regulis algebraicis, liber unus.  Joh. Petreius, 2011.
  * [32] M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, “Closed loop steering of unicycle like vehicles via lyapunov techniques,” IEEE Robotics & Automation Magazine, vol. 2, no. 1, pp. 27–35, 1995.
  * [33] J. Cortés and M. Egerstedt, “Coordinated control of multi-robot systems: A survey,” SICE Journal of Control, Measurement, and System Integration, vol. 10, no. 6, pp. 495–503, 2017.