00 XX 1234567 XX September 2021 XX October 2021 XX November 2021
OJCSYS.2021.Doi Number

Article Category

CORRESPONDING AUTHOR: Zhichao Li (e-mail<EMAIL_ADDRESS>This work was
supported by NSF RI IIS-2007141 and NSF CCF-2112665 (TILOS).

# Robust and Safe Autonomous Navigation for Systems with Learned SE(3)
Hamiltonian Dynamics

Zhichao Li (Student Member  IEEE)  Thai Duong (Student Member  IEEE)  Nikolay
Atanasov (Member  IEEE)

(2021)

###### Abstract

Stability and safety are critical properties for successful deployment of
automatic control systems. As a motivating example, consider autonomous mobile
robot navigation in a complex environment. A control design that generalizes
to different operational conditions requires a model of the system dynamics,
robustness to modeling errors, and satisfaction of safety constraints, such as
collision avoidance. This paper develops a neural ordinary differential
equation network to learn the dynamics of a Hamiltonian system from trajectory
data. The learned Hamiltonian model is used to synthesize an energy-shaping
passivity-based controller and analyze its _robustness_ to uncertainty in the
learned model and its _safety_ with respect to constraints imposed by the
environment. Given a desired reference path for the system, we extend our
design using a virtual reference governor to achieve tracking control. The
governor state serves as a regulation point that moves along the reference
path adaptively, balancing the system energy level, model uncertainty bounds,
and distance to safety violation to guarantee robustness and safety. Our
Hamiltonian dynamics learning and tracking control techniques are demonstrated
on simulated hexarotor and quadrotor robots navigating in cluttered 3D
environments.

constrained control, physics-constrained learning, safe learning for control

##  1 Introduction

Designing controllers that guarantee system stability and handle safety
constraints is an important problem in safety-critical applications of
automatic control systems, including autonomous transportation [1, 2], robot
locomotion [3], and medical robotics [4]. Safety depends on the system states,
governed by the system dynamics, and the environment constraints. This leads
to two requirements for designing provably safe controllers: an accurate model
of the system dynamics and the satisfaction of safety constraints.

The first requirement has motivated data-driven dynamics learning approaches,
utilizing machine learning techniques, such as Gaussian process (GP)
regression [5, 6, 7] and neural networks [8, 9]. For physical systems, recent
works [10, 11, 12] design the model architecture to impose a Lagrangian or
Hamiltonian formulation of the dynamics [13, 14], which a black-box model
might struggle to infer. For Lagrangian dynamics, Lutter et al. [10] use
neural networks to represent the mass and potential energy in the Euler-
Lagrange equations of motion. Meanwhile, Zhong et al. [11] use a
differentiable neural ODE solver [15] to predict state trajectories of a
Hamiltonian dynamics model, encoding Hamilton’s equations of motion. A
trajectory loss function is back-propagated through the ODE solver to update
the Hamiltonian model parameters. Our prior work [12] extends the neural ODE
Hamiltonian formulation by imposing $SE(3)$ constraints to capture the
kinematic evolution of rigid-body systems, such as ground or aerial robots. A
Hamiltonian-based model architecture also allows the design of stable
regulation or tracking controllers by energy shaping [11, 16, 12].
Interconnection and damping assignment passivity-based control (IDA-PBC) [17],
one of the main approaches for energy shaping, injects additional energy into
the system via the control input to achieve a desired total energy, which is
minimized at a desired regulation point. Instead of learning robot dynamics in
continuous time, Saemundsson et al. [18] design a variational integrator
network to learn discrete-time Lagrangian dynamics. Havens and Chowdhary [19]
extend it by including control input in the model and use model predictive
control for stabilization.

The second requirement, ensuring satisfaction of safety constraints, has
gained significant attention in planning and control. Model predictive control
(MPC) methods [20, 21, 22, 23] include safety constraints in an optimization
problem, which is typically solved by discretizing time and linearizing the
system dynamics. Reachability-based techniques [24, 25, 26, 27] are directly
applicable to nonlinear systems and offer strong safety guarantees but many
require solving a Hamilton-Jacobi partial differential equation (PDE) or sum-
of-squares optimization program. This is computationally challenging,
especially for high-dimensional systems, and may require system decomposition
techniques [28]. Control barrier functions (CBFs) [1, 29, 30] offer an elegant
approach to encode safety constraints. For a control-affine nonlinear system,
a CBF constraint is affine in the control input, allowing safe control
synthesis with quadratic programming (QP) [30]. However, constructing valid
CBFs that guarantee the feasibility of the QP problem is challenging [31, 32].
Given a stabilizing controller, reference governor techniques [33, 34, 35] use
a virtual governor system to introduce safety constraints based on the
Lyapunov function of the closed-loop system. Recent work [36, 37] achieves
safe trajectory tracking in unknown environments but is limited to linear or
feedback-linearizable systems.

Safe control synthesis with a learned model of the system dynamics needs to
account for the model estimation error between the learned and the ground-
truth dynamics [38]. Model uncertainty may be viewed as a disturbance, applied
to the learned system, and handled using robust or adaptive control techniques
[39, 40, 41, 42, 43, 44]. Safety constraint satisfaction in the presence of
model uncertainty can be achieved using robust MPC [45, 46, 7, 47], ${\cal
L}$-1 adaptive control [40], or model reference adaptive control [48] that
tracks the trajectory of a reference model and compensates for model
uncertainty. For example, Hewing et al. [7] propose an MPC technique that
trains a Gaussian Process (GP) model of the system dynamics and use the GP
uncertainty to introduce probabilistic safety constraints in the MPC
optimization. Robust controllers for Hamiltonian systems may also be developed
using the IDA-PBC approach [43, 49, 50]. Most techniques have considered
systems with states defined in Euclidean space and cannot handle manifold
constraints, e.g., due to the orientation kinematics of a mobile robot. For
quadrotors, Lee et al. [41] estimate disturbances from the tracking error and
design a robust geometric controller but do not consider safety constraints.

In this paper, we consider both dynamics model learning and safe control
synthesis for rigid-body systems, whose states include position, orientation,
and generalized velocity. We assume that system has an unknown dynamics model
but, as a physical system, it satisfies Hamilton’s equations of motion over
the $SE(3)$ manifold of positions and orientations. Given state-control
trajectories, from past experiments or collected by a human operator, we seek
to learn the system dynamics and design a tracking control law that handles
safety constraints, e.g., obtained from distance measurements to obstacles in
the environment. In our preliminary work [51], we learn a translation-
equivariant Hamiltonian model of the system dynamics using a physics-guided
neural ODE network [12]. We use the Hamiltonian model to synthesize an energy-
shaping geometric tracking controller. The total energy of the system serves
as a Lyapunov function and enables us to enforce safety constraints during
trajectory tracking using a reference governor to regulate the difference
between the system energy and the distance to safety violation. However, our
preliminary work [51] uses the learned Hamiltonian model as the ground-truth
dynamics and ignores the model estimation error in the control design. In this
paper, we capture the estimation error as a bounded disturbance applied to the
learned system and develop a robust safe tracking controller that takes the
disturbance into account in the design of the reference governor. Our
Hamiltonian dynamics learning and tracking control techniques are compared to
a GP MPC technique [7] and are demonstrated in a 3D environment using a
simulated hexarotor robot to achieve collision-free autonomous navigation.

In summary, the contribution of this work is a tracking control design for
Hamiltonian systems with learned dynamics, which achieves _robustness_ to
model estimation errors and _safety_ with respect to state constraints.

##  2 Problem Statement

Consider a rigid body with position $\mathbf{p}\in\mathbb{R}^{3}$, orientation
$\mathbf{R}\in SO(3)$, body-frame linear velocity
$\mathbf{v}\in\mathbb{R}^{3}$, and body-frame angular velocity
${\boldsymbol{\omega}}\in\mathbb{R}^{3}$. Let
${\mathfrak{q}}=[\mathbf{p}^{\top}\;\;\mathbf{r}_{1}^{\top}\;\;\mathbf{r}_{2}^{\top}\;\;\mathbf{r}_{3}^{\top}]^{\top}$
$\in SE(3)$ denote the body’s generalized coordinates, where $\mathbf{r}_{1}$,
$\mathbf{r}_{2}$, $\mathbf{r}_{3}\in\mathbb{R}^{3}$ are the rows of the
rotation matrix $\mathbf{R}$. Let
${\boldsymbol{\zeta}}=[\mathbf{v}^{\top}\;\;{\boldsymbol{\omega}}^{\top}]^{\top}\in\mathbb{R}^{6}$
denote the body’s generalized velocity. The generalized momentum
${\mathfrak{p}}$ of the body is defined as:

${\mathfrak{p}}=\mathbf{M}({\mathfrak{q}}){\boldsymbol{\zeta}}\in\mathbb{R}^{6},$
(1)

where $\mathbf{M}({\mathfrak{q}})\succ 0$ is the positive-definite generalized
mass matrix. Let $\mathbf{x}=({\mathfrak{q}},{\mathfrak{p}})\in T^{*}SE(3)$
denote the state of the rigid body system on the cotangent bundle $T^{*}SE(3)$
of the $SE(3)$ manifold. The Hamiltonian,
$\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})$, captures the
total energy of the system as the sum of the kinetic energy ${\cal
T}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})=\frac{1}{2}\mathbf{{\mathfrak{p}}}^{\top}\mathbf{M}^{-1}({\mathfrak{q}})\mathbf{{\mathfrak{p}}}$
and the potential energy ${\cal U}({\mathfrak{q}})$:

$\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})={\cal
T}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})+{\cal
U}(\mathbf{{\mathfrak{q}}}).$ (2)

The evolution of the state $\mathbf{x}$ is governed by Hamilton’s equations of
motion [52]:

$\displaystyle\dot{\mathbf{x}}$
$\displaystyle=\mathbf{f}(\mathbf{x})+\mathbf{G}(\mathbf{x})\mathbf{u},\qquad\qquad\mathbf{x}(t_{0})=\mathbf{x}_{0},$
(3) $\displaystyle=\begin{bmatrix}\bf 0&\mathbf{{\mathfrak{q}}}^{\times}\\\
-\mathbf{{\mathfrak{q}}}^{\times\top}&\mathbf{{\mathfrak{p}}}^{\times}\end{bmatrix}\begin{bmatrix}\nabla_{\mathfrak{q}}\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})\\\
\nabla_{\mathfrak{p}}\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})\end{bmatrix}+\begin{bmatrix}\bf
0\\\ \mathbf{B}(\mathbf{{\mathfrak{q}}})\end{bmatrix}\mathbf{u}$

where $\mathbf{u}\in\mathbb{R}^{6}$ is the control input, e.g. force and
torque or motor speeds for a UAV system,
$\mathbf{B}(\mathbf{{\mathfrak{q}}})\in\mathbb{R}^{6\times 6}$ is an input
gain matrix, and the operators $\mathbf{{\mathfrak{q}}}^{\times}$,
$\mathbf{{\mathfrak{p}}}^{\times}$ are defined as:

$\mathbf{{\mathfrak{q}}}^{\times}=\begin{bmatrix}\mathbf{R}^{\top}\\!\\!\\!\\!&\bf
0&\bf 0&\bf 0\\\ \bf
0&\hat{\mathbf{r}}_{1}^{\top}&\hat{\mathbf{r}}_{2}^{\top}&\hat{\mathbf{r}}_{3}^{\top}\end{bmatrix}^{\top}\\!\\!\\!\\!,\quad\mathbf{{\mathfrak{p}}}^{\times}=\begin{bmatrix}\mathbf{{\mathfrak{p}}}_{\mathbf{v}}\\\
\mathbf{{\mathfrak{p}}}_{{\boldsymbol{\omega}}}\end{bmatrix}^{\times}\\!\\!\\!\\!=\begin{bmatrix}\bf
0&\hat{\mathbf{{\mathfrak{p}}}}_{\mathbf{v}}\\\
\hat{\mathbf{{\mathfrak{p}}}}_{\mathbf{v}}&\hat{\mathbf{{\mathfrak{p}}}}_{{\boldsymbol{\omega}}},\end{bmatrix},$

where the hat map $\hat{\mathbf{w}}$ for $\mathbf{w}\in\mathbb{R}^{3}$ is:

$\hat{\mathbf{w}}=\begin{bmatrix}0&-w_{3}&w_{2}\\\ w_{3}&0&-w_{1}\\\
-w_{2}&w_{1}&0\end{bmatrix}.$

The Hamiltonian dynamics model in (3) can be extended to include energy
dissipation in a port-Hamiltonian formulation [17] such as friction or drag
forces [53]. However, for clarity of the control design, we leave this for
future work.

We consider the case that the parameters of the Hamiltonian dynamics model in
(3), including the mass $\mathbf{M}({\mathfrak{q}})$, the potential energy
${\cal U}({\mathfrak{q}})$, and the input matrix $\mathbf{B}({\mathfrak{q}})$,
are unknown. Instead, we are given a trajectory dataset ${\cal
D}=\\{t^{(i)}_{0:N},{\mathfrak{q}}^{(i)}_{0:N},{\boldsymbol{\zeta}}^{(i)}_{0:N},\mathbf{u}_{0:N-1}^{(i)}\\}_{i=1}^{D}$
consisting of $D$ sequences of generalized coordinates and velocities
$({\mathfrak{q}}^{(i)}_{0:N},{\boldsymbol{\zeta}}^{(i)}_{0:N})$ at times
$t_{0}^{(i)}<t_{1}^{(i)}<\ldots<t_{N}^{(i)}$, collected by applying a constant
control input $\mathbf{u}_{n}^{(i)}$ to the system with initial condition
$({\mathfrak{q}}^{(i)}_{n},{\boldsymbol{\zeta}}^{(i)}_{n})$ for
$t\in[t_{n},t_{n+1})$ and $n=0,\ldots,N-1$. Our objective is to learn the
system dynamics from the data set ${\cal D}$ and design a control policy
$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x})$ such that the system follows a
desired reference path without violating safety constraints. Let ${\cal
O}\subset\mathbb{R}^{3}$ and ${\cal F}\coloneqq\mathbb{R}^{3}\setminus{\cal
O}$ denote the unsafe (obstacle) set and the safe (obstacle-free) set,
respectively. Denote the interior of ${\cal F}$ as $\text{int}({\cal F})$. We
assume that ${\cal O}$ is not known a priori but the distance
$\bar{d}(\mathbf{p},{\cal O})$ from the system’s position $\mathbf{p}$ to
${\cal O}$ can be sensed with a limited sensing range $d_{max}>0$:

$\bar{d}(\mathbf{p},{\cal O})\coloneqq\min\left\\{d(\mathbf{p},{\cal
O}),d_{max}\right\\},$ (4)

where $d(\mathbf{p},{\cal O})\coloneqq\inf_{\mathbf{a}\in{\cal
O}}\lVert\mathbf{p}-\mathbf{a}\rVert$ denotes the Euclidean distance from
$\mathbf{p}$ to the set ${\cal O}$. The safe tracking control problem
considered in this paper is summarized below.

###### Problem 1

Let ${\cal
D}=\\{t^{(i)}_{0:N},{\mathfrak{q}}^{(i)}_{0:N},{\boldsymbol{\zeta}}^{(i)}_{0:N},\mathbf{u}_{0:N-1}^{(i)}\\}_{i=1}^{D}$
be a training dataset of state-control trajectories obtained from a rigid-body
system with unknown Hamiltonian dynamics in (3). Let
$\mathbf{r}:\left[0,1\right]\mapsto\text{Int}{\rm Phys.~{}Rev.~{}Lett.}{{\cal
F}}$ be a continuous function specifying a desired position reference path for
the system. Assume that the reference path starts at the initial position at
time $t_{0}$, i.e., $\mathbf{r}(0)=\mathbf{p}(t_{0})\in\text{Int}{\rm
Phys.~{}Rev.~{}Lett.}{{\cal F}}$. Using local distance observations
$\bar{d}(\mathbf{p}(t),{\cal O})$ of the unsafe set ${\cal O}$, design a
control policy ${\boldsymbol{\pi}}:T^{*}SE(3)\mapsto\mathbb{R}^{6}$ so that
the position $\mathbf{p}(t)$ of the closed-loop system with control law
$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x})$ converges asymptotically to
$\mathbf{r}(1)$, while remaining safe, i.e., $\mathbf{p}(t)\in{\cal F},\forall
t\geq t_{0}$.

##  3 Learning $SE(3)$ Hamiltonian Dynamics from Data

In this section, we design a dynamics model that can be learned from a
previously collected trajectory dataset, e.g., obtained from manual control,
and is sufficiently general to represent different mobile robots, such as cars
and drones. We describe how to learn Hamiltonian dynamics from the dataset
${\cal
D}=\\{t^{(i)}_{0:N},{\mathfrak{q}}^{(i)}_{0:N},{\boldsymbol{\zeta}}^{(i)}_{0:N},\mathbf{u}_{0:N-1}^{(i)}\\}_{i=1}^{D}$,
described in Sec. 2, using translation-equivariant Hamiltonian-based neural
ODE networks [12]. The mass $\mathbf{M}({\mathfrak{q}})$, the potential energy
${\cal U}({\mathfrak{q}})$ and the input gain $\mathbf{B}({\mathfrak{q}})$ are
approximated by neural networks. We show that the model estimation errors
caused by the trained neural networks can be considered as a disturbance
applied on the learned system.

###  3.1 Translation-equivariant $SE(3)$ Hamiltonian dynamics learning

Since the system dynamics does not change if we shift the position
$\mathbf{p}$ to any points in the world frame, we offset the trajectories in
the dataset ${\cal D}$ so that they start from the position $\bf 0$ and learn
the system dynamics well around the origin. This is sufficient for
stabilization task, e.g. using the controller design in Sec. 4, because
driving the system from state $\mathbf{x}$ with position $\mathbf{p}$ to a
desired state $\mathbf{x}^{*}$ with position $\mathbf{p}^{*}$ is the same as
driving the system from the state $\mathbf{x}$ with position $\bf 0$ to a
desired state $\mathbf{x}^{*}$ with offset position
$\mathbf{p}^{*}-\mathbf{p}$.

Since the momentum ${\mathfrak{p}}$ is not directly available from the dataset
${\cal D}$, we use the time derivative of the generalized velocity, derived
from (1):

$\dot{{\boldsymbol{\zeta}}}={\rm
Phys.~{}Rev.~{}Lett.}{\frac{d}{dt}\mathbf{M}^{-1}(\mathbf{{\mathfrak{q}}})}\mathbf{{\mathfrak{p}}}+\mathbf{M}^{-1}(\mathbf{{\mathfrak{q}}})\dot{\mathbf{{\mathfrak{p}}}}.$
(5)

Eq. (3) and (5) describe the Hamiltonian dynamics of the generalized
coordinates and velocities with unknown inverse generalized mass matrix
$\mathbf{M}^{-1}({\mathfrak{q}})$, input matrix $\mathbf{B}({\mathfrak{q}})$,
and potential energy ${\cal U}({\mathfrak{q}})$, for which we aim to
approximate by three neural networks
$\mathbf{M}_{\boldsymbol{\theta}}^{-1}({\mathfrak{q}}),\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$
and ${\cal U}_{\boldsymbol{\theta}}({\mathfrak{q}})$, respectively, with
parameters ${\boldsymbol{\theta}}$.

To optimize for the parameters ${\boldsymbol{\theta}}$, we use the
Hamiltonian-based neural ODE framework that encodes the Hamiltonian dynamics
(3) and (5) with
$\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}}),\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$
and ${\cal U}_{\boldsymbol{\theta}}({\mathfrak{q}})$ in the network structure
(Fig. 1). The forward pass rolls out the dynamics
$\bar{\mathbf{f}}_{\boldsymbol{\theta}}$ described by (3) and (5) with the
neural networks
$\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}}),\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$
and ${\cal U}_{\boldsymbol{\theta}}({\mathfrak{q}})$ using a neural ODE solver
([15]) with initial state
$({{\mathfrak{q}}}^{(i)}_{n},{{\boldsymbol{\zeta}}}^{(i)}_{n})$. We obtain a
predicted state
$(\bar{{\mathfrak{q}}}^{(i)}_{n+1},\bar{{\boldsymbol{\zeta}}}^{(i)}_{n+1})$ at
times $t_{n+1}^{(i)}$ for each $n=0,\ldots,N-1$ and $i=1,\ldots,D$ as:

$(\bar{{\mathfrak{q}}}^{(i)}_{n+1},\bar{{\boldsymbol{\zeta}}}^{(i)}_{n+1})=\text{ODESolver}{\rm
Phys.~{}Rev.~{}Lett.}{({{\mathfrak{q}}}^{(i)}_{n},{{\boldsymbol{\zeta}}}^{(i)}_{n}),\bar{\mathbf{f}},t_{n+1}^{(i)}-t_{n}^{(i)};{\boldsymbol{\theta}}}.$

The loss function is defined as ${\cal
L}=\sum_{i=1}^{D}\sum_{n=1}^{N}c({\mathfrak{q}}^{(i)}_{n},{\boldsymbol{\zeta}}^{(i)}_{n},\bar{{\mathfrak{q}}}^{(i)}_{n},\bar{{\boldsymbol{\zeta}}}^{(i)}_{n})$
where the distance metric $c$ is defined as the sum of position, orientation,
and velocity errors on the tangent bundle $TSE(3)$:

$c{\rm
Phys.~{}Rev.~{}Lett.}{{\mathfrak{q}},{\boldsymbol{\zeta}},\bar{{\mathfrak{q}}},\bar{{\boldsymbol{\zeta}}}}=c_{\mathbf{p}}(\mathbf{p},\bar{\mathbf{p}})+c_{\mathbf{R}}(\mathbf{R},\bar{\mathbf{R}})+c_{{\boldsymbol{\zeta}}}({\boldsymbol{\zeta}},\bar{{\boldsymbol{\zeta}}}),$
(6)

with the position error
$c_{\mathbf{p}}(\mathbf{p},\bar{\mathbf{p}})=\|\mathbf{p}-\bar{\mathbf{p}}\|^{2}_{2}$,
the velocity error
$c_{{\boldsymbol{\zeta}}}({\boldsymbol{\zeta}},\bar{{\boldsymbol{\zeta}}})=\|{\boldsymbol{\zeta}}-\bar{{\boldsymbol{\zeta}}}\|^{2}_{2}$,
and the rotation error
$c_{\mathbf{R}}(\mathbf{R},\bar{\mathbf{R}})=\|\left(\log(\bar{\mathbf{R}}\mathbf{R}^{\top})\right)^{\vee}\|_{2}^{2}$.
The $\log$-map $\log(\cdot):SE(3)\mapsto\mathfrak{so}(3)$ returns a skew-
symmetric matrix in $\mathfrak{so}(3)$ from a rotation matrix in $SE(3)$, and
the $\vee$-map $(\cdot)^{\vee}:\mathfrak{so}(3)\mapsto\mathbb{R}^{3}$ is the
inverse of the hat map $\hat{(\cdot)}$ in Sec. 2.

The network parameters ${\boldsymbol{\theta}}$ are optimized using gradient
descent by back-propagating the gradient $\nabla_{\boldsymbol{\theta}}{\cal
L}$ of the loss through the neural ODE solver efficiently using adjoint method
[15]. Specifically, let
$\mathbf{a}=\nabla_{{\mathfrak{q}},{\boldsymbol{\zeta}}}{\cal L}$ be the
adjoint state and $\mathbf{s}={\rm
Phys.~{}Rev.~{}Lett.}{({\mathfrak{q}},{\boldsymbol{\zeta}}),\mathbf{a},\nabla_{\boldsymbol{\theta}}{\cal
L}}$ be the augmented state. The augmented state dynamics are [15]:

$\dot{\mathbf{s}}=\bar{\mathbf{f}}_{\mathbf{s}}={\rm
Phys.~{}Rev.~{}Lett.}{\bar{\mathbf{f}}_{{\boldsymbol{\theta}}},-\mathbf{a}^{\top}\nabla_{{\mathfrak{q}},{\boldsymbol{\zeta}}}\bar{\mathbf{f}}_{{\boldsymbol{\theta}}},-\mathbf{a}^{\top}\nabla_{\boldsymbol{\theta}}\bar{\mathbf{f}}_{{\boldsymbol{\theta}}}}.$
(7)

We obtain the gradient $\nabla_{\boldsymbol{\theta}}{\cal L}$ by a single call
to a reverse-time ODE solver starting from
$\mathbf{s}_{n+1}=\mathbf{s}(t_{n+1})$:

$\mathbf{s}_{0}=\left(\bar{\mathbf{x}}_{0},\mathbf{a}_{0},\nabla_{\boldsymbol{\theta}}{\cal
L}\right)=\text{ODESolver}(\mathbf{s}_{n+1},\bar{\mathbf{f}}_{\mathbf{s}},t_{n+1}-t_{n}),$
(8)

for $n=0,\ldots,N-1$, and update the parameters ${\boldsymbol{\theta}}$ using
gradient descent. Please refer to [15] for more details.

Figure 1: Architecture of $SE(3)$ Hamiltonian neural ODE network.

###  3.2 Model estimation error as a disturbance

Via the training process described in Sec. 3.1, we approximate the ground
truth mass $\tilde{\mathbf{M}}({\mathfrak{q}})$, potential energy
$\tilde{{\cal U}}({\mathfrak{q}})$ and input gain matrix
$\tilde{\mathbf{B}}({\mathfrak{q}})$ with the learned mass
$\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}})=\tilde{\mathbf{M}}({\mathfrak{q}})+\Delta\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}})$,
potential energy ${\cal U}({\mathfrak{q}})=\tilde{{\cal
U}}({\mathfrak{q}})+\Delta{\cal U}_{\boldsymbol{\theta}}({\mathfrak{q}})$, and
input gain
$\mathbf{B}({\mathfrak{q}})=\tilde{\mathbf{B}}({\mathfrak{q}})+\Delta\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$
where $\Delta\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}}),\Delta{\cal
U}_{\boldsymbol{\theta}}({\mathfrak{q}})$, and
$\Delta\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$ are the estimation
errors. We drop the subscript ${\boldsymbol{\theta}}$ to simplify the
notations. The generalized coordinates ${\mathfrak{q}}$ and the ground-truth
momentum
$\tilde{{\mathfrak{p}}}:=\tilde{\mathbf{M}}({\mathfrak{q}}){\boldsymbol{\zeta}}$,
satisfy the Hamiltonian dynamics (3):

$\displaystyle\dot{{\mathfrak{q}}}$
$\displaystyle={\mathfrak{q}}^{\times}\nabla_{\tilde{{\mathfrak{p}}}}\mathcal{\tilde{H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})={\mathfrak{q}}^{\times}{\boldsymbol{\zeta}}$
(9) $\displaystyle\dot{\tilde{{\mathfrak{p}}}}$
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{\tilde{H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})+\tilde{{\mathfrak{p}}}^{\times}\nabla_{\mathfrak{p}}\mathcal{\tilde{H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})+\tilde{\mathbf{B}}({\mathfrak{q}})\mathbf{u}$
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{\tilde{H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})+\tilde{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}+\tilde{\mathbf{B}}({\mathfrak{q}})\mathbf{u},$

with the ground-truth Hamiltonian

$\tilde{\mathcal{H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})=\frac{1}{2}\tilde{\mathbf{{\mathfrak{p}}}}^{\top}\tilde{\mathbf{M}}^{-1}(\mathbf{{\mathfrak{q}}})\tilde{\mathbf{{\mathfrak{p}}}}+\tilde{{\cal
U}}(\mathbf{{\mathfrak{q}}})=\frac{1}{2}{\boldsymbol{\zeta}}^{\top}\tilde{\mathbf{M}}(\mathbf{{\mathfrak{q}}}){\boldsymbol{\zeta}}+\tilde{{\cal
U}}(\mathbf{{\mathfrak{q}}}).$ (10)

Meanwhile, for the generalized coordinates ${\mathfrak{q}}$ and the momentum
${\mathfrak{p}}:=\mathbf{M}({\mathfrak{q}}){\boldsymbol{\zeta}}$, the
Hamiltonian dynamics is learned from data and of the form:

$\displaystyle\dot{{\mathfrak{q}}}$
$\displaystyle={\mathfrak{q}}^{\times}\nabla_{{{\mathfrak{p}}}}\mathcal{{H}}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})={\mathfrak{q}}^{\times}{\boldsymbol{\zeta}}$
(11) $\displaystyle\dot{{{\mathfrak{p}}}}$
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{{H}}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})+{{\mathfrak{p}}}^{\times}\nabla_{\mathfrak{p}}\mathcal{{H}}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})+{\mathbf{B}}({\mathfrak{q}})\mathbf{u}$
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{{H}}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})+{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}+{\mathbf{B}}({\mathfrak{q}})\mathbf{u},$

with the learned Hamiltonian

$\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})=\frac{1}{2}{\boldsymbol{\zeta}}^{\top}\mathbf{M}(\mathbf{{\mathfrak{q}}}){\boldsymbol{\zeta}}+{\cal
U}(\mathbf{{\mathfrak{q}}})=\tilde{{\cal
H}}(\mathbf{{\mathfrak{q}}},\tilde{\mathbf{{\mathfrak{p}}}})+\Delta{\cal
H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}}),$

and its estimation error $\Delta{\cal
H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})=\frac{1}{2}{\boldsymbol{\zeta}}^{\top}\Delta\mathbf{M}(\mathbf{{\mathfrak{q}}}){\boldsymbol{\zeta}}+\Delta{\cal
U}(\mathbf{{\mathfrak{q}}})$.

However, the learned dynamics (11) is only an approximation of the actual
dynamics for $({\mathfrak{q}},{\mathfrak{p}})$. While the dynamics of
${\mathfrak{q}}$ does not change, the actual dynamics of the learned momentum,
${\mathfrak{p}}=\mathbf{M}({\mathfrak{q}}){\boldsymbol{\zeta}}=\tilde{{\mathfrak{p}}}+\Delta{\mathfrak{p}}$,
where
$\Delta{\mathfrak{p}}=\Delta\mathbf{M}({\mathfrak{q}}){\boldsymbol{\zeta}}$,
is derived from (9) as follows:

$\displaystyle\dot{{\mathfrak{p}}}$
$\displaystyle=\dot{\tilde{{\mathfrak{p}}}}+\dot{\Delta{\mathfrak{p}}}$ (12)
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})+{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}+{\mathbf{B}}({\mathfrak{q}})\mathbf{u}$
$\displaystyle\quad+{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}{\rm
Phys.~{}Rev.~{}Lett.}{\Delta\mathcal{H}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})}-\Delta{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}-\Delta{\mathbf{B}}({\mathfrak{q}})\mathbf{u}+\dot{\Delta{\mathfrak{p}}}.$
$\displaystyle=-{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}\mathcal{H}(\mathbf{{\mathfrak{q}}},\mathbf{{\mathfrak{p}}})+{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}+{\mathbf{B}}({\mathfrak{q}})\mathbf{u}+\mathbf{d}_{1},$

where the force

$\mathbf{d}_{1}~{}:=~{}{\mathfrak{q}}^{\times\top}\nabla_{\mathfrak{q}}{\rm
Phys.~{}Rev.~{}Lett.}{\Delta\mathcal{H}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})}-\Delta{{\mathfrak{p}}}^{\times}{\boldsymbol{\zeta}}-\Delta{\mathbf{B}}({\mathfrak{q}})\mathbf{u}+\dot{\Delta{\mathfrak{p}}}$
(13)

represents the effect of the model errors
$\Delta\mathbf{M}({\mathfrak{q}}),\Delta{\cal U}({\mathfrak{q}})$, and
$\Delta\mathbf{B}({\mathfrak{q}})$ and is considered as a disturbance applied
to the learned system (11). To improve the error $\mathbf{d}_{1}$ with respect
to the position $\mathbf{p}$, we enforce translation-equivariance in the
neural ODE model, as described in Sec. III.A, and learn the model well around
the origin. This allows us to offset any position $\mathbf{p}$ to the well-
learned region around the origin. To reduce the model error with respect to
orientation, we collect a training dataset that covers different regions of
roll, pitch, and yaw angles, e.g. by manually driving a UAV to different
desired positions and yaw angles. A promising approach to estimate the
disturbance magnitude is to employ a Bayesian formulation of the neural ODE
network used to learn the dynamics model. A Bayesian model will provide a
posterior distribution, rather than point estimates, for the model parameters
(i.e. $\mathbf{M}^{-1}({\mathfrak{q}})$, $\mathbf{B}({\mathfrak{q}})$, and
${\cal U}({\mathfrak{q}})$), whose variance can be used to obtain parameter
error bounds and, in turn, a disturbance bound. Bayesian neural network models
that can be used for dynamics learning include Bayesian neural ODE networks
[54, 55], neural stochastic differential equation (SDE) networks [56], or
Gaussian-process ODEs [57]. This motivates analyzing the robustness of our
control design in Sec. 4 to the disturbance $\mathbf{d}_{1}$ caused by the
model errors.

##  4 Stabilization of Hamiltonian Dynamics with Matched Disturbances

As discussed in Sec. 3.3.2, due to estimation errors in the dynamics learning
process, the learned system model satisfies Hamilton’s equations of motion in
(3) subject to a matched disturbance signal
$\mathbf{d}_{1}:\mathbb{R}\mapsto\mathbb{R}^{6}$:

$\displaystyle{\begin{bmatrix}\dot{{\mathfrak{q}}}\\\ \dot{{\mathfrak{p}}}\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{0}&{\mathfrak{q}}^{\times}\\\
-{\mathfrak{q}}^{\times\top}\\!\\!&{\mathfrak{p}}^{\times}\end{bmatrix}\begin{bmatrix}\nabla_{\mathfrak{q}}\mathcal{H}({\mathfrak{q}},{\mathfrak{p}})\\\
\nabla_{\mathfrak{p}}\mathcal{H}({\mathfrak{q}},{\mathfrak{p}})\end{bmatrix}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{B}({\mathfrak{q}})\end{bmatrix}\mathbf{u}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{d}_{1}\end{bmatrix}.}$

(14)

We consider a passivity-based stabilizing controller for (14), and analyze its
robustness with respect to the disturbance signal $\mathbf{d}_{1}$ and its
safety with respect to the obstacle set ${\cal O}$.

###  4.1 Passivity-based control

Consider a desired regulation point
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},{\mathfrak{p}}^{*})$ for the system in
(14) with generalized coordinates
${\mathfrak{q}}^{*}=(\mathbf{p}^{*},\mathbf{R}^{*})$ and momentum
${\mathfrak{p}}^{*}=0$. Since the Hamiltonian ${\cal H}(\mathbf{x})$ may not
have a minimum at $\mathbf{x}^{*}$, the control signal $\mathbf{u}$ in (14)
should be designed to inject additional energy ${\cal
H}_{a}(\mathbf{x},\mathbf{x}^{*})$ into system and achieve a desired
Hamiltonian ${\cal H}_{d}(\mathbf{x},\mathbf{x}^{*})={\cal
H}(\mathbf{x})+{\cal H}_{a}(\mathbf{x},\mathbf{x}^{*})$, which is minimized at
$\mathbf{x}^{*}$. This is the approach followed by interconnection and damping
assignment passivity-based control (IDA-PBC) [58]. Let
$\mathbf{x}_{e}=({\mathfrak{q}}_{e},{\mathfrak{p}}_{e})$ denote the error in
generalized coordinates and momentum:

$\displaystyle\mathbf{R}_{e}$
$\displaystyle=\mathbf{R}^{*\top}\mathbf{R}=\begin{bmatrix}\mathbf{r}_{e1}&\mathbf{r}_{e2}&\mathbf{r}_{e3}\end{bmatrix}^{\top}$
$\displaystyle\mathbf{p}_{e}$ $\displaystyle=\mathbf{p}-\mathbf{p}^{*}$ (15)
$\displaystyle{\mathfrak{q}}_{e}$
$\displaystyle=\begin{bmatrix}\mathbf{p}_{e}^{\top}&\mathbf{r}_{e1}^{\top}&\mathbf{r}_{e2}^{\top}&\mathbf{r}_{e3}^{\top}\end{bmatrix}^{\top}$
$\displaystyle{\mathfrak{p}}_{e}$
$\displaystyle={\mathfrak{p}}-{\mathfrak{p}}^{*}.$

A possible choice of ${\cal H}_{d}(\mathbf{x},\mathbf{x}^{*})$, minimized at
$\mathbf{x}=\mathbf{x}^{*}$, is:

$\displaystyle{\cal H}_{d}(\mathbf{x},\mathbf{x}^{*})$ $\displaystyle={\cal
T}({\mathfrak{q}}_{e},{\mathfrak{p}}_{e})+{\cal U}_{d}({\mathfrak{q}}_{e})$
(16)
$\displaystyle=\frac{1}{2}{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}({\mathfrak{q}}_{e}){\mathfrak{p}}_{e}+\frac{k_{\mathbf{p}}}{2}\lVert\mathbf{p}_{e}\rVert^{2}+\frac{k_{\mathbf{R}}}{2}\operatorname{tr}(\mathbf{I}-\mathbf{R}_{e}),$

where $k_{\mathbf{p}}$ and $k_{\mathbf{R}}$ are positive scalars.

The IDA-PBC method [12, 58] designs a controller
$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x},\mathbf{x}^{*})$ such that the
closed-loop dynamics of the system in (14) are governed by the desired
Hamiltonian in (16) as:

$\begin{bmatrix}\dot{\mathbf{{\mathfrak{q}}}}_{e}\\\
\dot{\mathbf{{\mathfrak{p}}}}_{e}\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{0}&\mathbf{J}(\mathbf{x},\mathbf{x}^{*})\\\
-\mathbf{J}(\mathbf{x},\mathbf{x}^{*})^{\top}&-\mathbf{K}_{d}\end{bmatrix}\begin{bmatrix}\nabla_{{\mathfrak{q}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})\\\ \nabla_{{\mathfrak{p}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})\end{bmatrix}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{d}\end{bmatrix},$ (17)

where the terms $\mathbf{J}(\mathbf{x},\mathbf{x}^{*})$, $\mathbf{K}_{d}$, and
$\mathbf{d}$ in the transformed dynamics depend on the control design. To
obtain the controller, one uses the relationship between $\mathbf{x}$ and
$\mathbf{x}_{e}$ in (15) to equate the dynamics in (14) and (17), leading to:

$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x},\mathbf{x}^{*})=\mathbf{B}^{\dagger}({\mathfrak{q}})\mathbf{b}(\mathbf{x},\mathbf{x}^{*}),$
(18)

where
$\mathbf{B}^{\dagger}({\mathfrak{q}})=(\mathbf{B}^{\top}({\mathfrak{q}})\mathbf{B}({\mathfrak{q}}))^{-1}\mathbf{B}^{\top}({\mathfrak{q}})$
is the pseudo-inverse of the input gain $\mathbf{B}({\mathfrak{q}})$ and:

$\mathbf{b}(\mathbf{x},\mathbf{x}^{*})=\Big{(}{\mathfrak{q}}^{\times\top}\nabla_{{\mathfrak{q}}}{\cal
H}(\mathbf{x})-{\mathfrak{p}}^{\times}\nabla_{\mathfrak{p}}{\cal
H}(\mathbf{x})\\\
-\mathbf{J}(\mathbf{x},\mathbf{x}^{*})^{\top}\nabla_{{\mathfrak{q}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})-\mathbf{K}_{d}\nabla_{{\mathfrak{p}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})\Big{)}$ (19)

with
$\mathbf{J}(\mathbf{x},\mathbf{x}^{*})\coloneqq\begin{bmatrix}\mathbf{R}^{\top}\\!\\!\\!\\!&\bf
0&\bf 0&\bf 0\\\ \bf
0&\hat{\mathbf{r}}_{e1}^{\top}&\hat{\mathbf{r}}_{e2}^{\top}&\hat{\mathbf{r}}_{e3}^{\top}\end{bmatrix}^{\top}.$
If the IDA-PBC matching equations [59],

$\mathbf{B}^{\perp}({\mathfrak{q}})\mathbf{b}(\mathbf{x},\mathbf{x}^{*})=0,$
(20)

are satisfied, where $\mathbf{B}^{\perp}({\mathfrak{q}})$ is a maximal-rank
left annihilator of $\mathbf{B}({\mathfrak{q}})$, i.e.,
$\mathbf{B}^{\perp}({\mathfrak{q}})\mathbf{B}({\mathfrak{q}})=\bf 0$, then the
controller in (18) achieves the desired closed-loop dynamics in (17) with
$\mathbf{d}=\mathbf{d}_{1}$, i.e., without introducing any extra disturbance.

If the matching equations (20) cannot be satisfied globally, i.e., the IDA-PBC
controller does not solve the system
$\mathbf{B}({\mathfrak{q}})\mathbf{u}=\mathbf{b}(\mathbf{x},\mathbf{x}^{*})$
exactly, then
${\boldsymbol{\pi}}(\mathbf{x}^{*},\mathbf{x})=\mathbf{B}^{\dagger}({\mathfrak{q}})\mathbf{b}(\mathbf{x},\mathbf{x}^{*})$
is a least-squares solution. In this case, the residual,

$\mathbf{d}_{2}\coloneqq{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{B}({\mathfrak{q}})\mathbf{B}^{\dagger}({\mathfrak{q}})-\mathbf{I}}\mathbf{b}(\mathbf{x},\mathbf{x}^{*}),$
(21)

is introduced as an additional matched disturbance:

$\mathbf{d}=\mathbf{d}_{1}+\mathbf{d}_{2}$ (22)

in the closed-loop dynamics in (17). Since the magnitude of $\mathbf{d}_{2}$
is proportional to that of $\mathbf{b}(\mathbf{x},\mathbf{x}^{*})$, it depends
on the desired regulation point $\mathbf{x}^{*}$. An underactuated quadrotor
system example is provided in Sec. 7.7.4.

In general, the matching equations (20) are nonlinear PDEs and can be solved
explicitly only for certain cases [59]. If $\mathbf{B}({\mathfrak{q}})$ is
invertible, i.e., the system in (14) is fully-actuated, then the solution in
(18) exists and is unique. For systems with underactuation degree $1$, the
matching equations may be reduced to ODEs with closed-form solution [60] or
solved with certain desired kinetic energy [61]. Yuksel et al. [62] solve the
matching equations specifically for stabilizing a quadrotor system, using
Euler angles instead of a rotation matrix. A survey on this topic is available
in [59].

###  4.2 Robustness analysis

In this section, we analyze the stability and robustness with respect to the
disturbance signal $\mathbf{d}$ in (22) of the IDA-PBC controller in (18).
Although the techniques we developed for dynamics learning in Sec. 3 and
control synthesis in Sec. 4.4.1 did not make any assumptions about the
Hamiltonian system in (14), our robustness and safety analysis that follows is
developed under two assumptions.

###### Assumption 1

The disturbance signal (22) is uniformly bounded, i.e.,
$\lVert\mathbf{d}\rVert\leq\delta_{\mathbf{d}}$ for some
$\delta_{\mathbf{d}}>0$.

###### Assumption 2

The generalized mass matrix is constant, i.e.,
$\mathbf{M}({\mathfrak{q}})\equiv\mathbf{M}$.

Without Assumption 1, it is not possible to provide any performance guarantees
for the control design because the disturbance $\mathbf{d}$ can have an
arbitrary effect on the evolution of the closed-loop system dynamics. The
disturbance magnitude bound $\delta_{\mathbf{d}}$ exists if we assume bounded
estimation errors, bounded velocity and acceleration, bounded
$\nabla_{\mathfrak{q}}{\rm
Phys.~{}Rev.~{}Lett.}{\Delta\mathcal{H}(\mathbf{{\mathfrak{q}}},{\mathbf{{\mathfrak{p}}}})}$,
and bounded control input $\mathbf{u}$ from the controller (18).

Our robustness analysis in Thm. 1 below constructs an ISS-Lyapunov function
[63] to handle the disturbance $\mathbf{d}$. Assumption 2 simplifies the proof
that we have a valid ISS-Lyapunov function. Extending the analysis to handle a
state-dependent mass $\mathbf{M}({\mathfrak{q}})$ is left for future work.

We simplify the error dynamics (17) by noting that:

$\displaystyle{\mathbf{e}(\mathbf{x},\mathbf{x}^{*})\coloneqq\mathbf{J}(\mathbf{x},\mathbf{x}^{*})^{\top}\nabla_{{\mathfrak{q}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})=\begin{bmatrix}k_{\mathbf{p}}\mathbf{R}^{\top}\mathbf{p}_{e}\\\
\frac{1}{2}k_{\mathbf{R}}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{R}_{e}-\mathbf{R}_{e}^{\top}}^{\vee}\end{bmatrix},}$

which leads to:

$\displaystyle\dot{{\mathfrak{q}}}_{e}$
$\displaystyle=\mathbf{J}(\mathbf{x},\mathbf{x}^{*})\mathbf{M}^{-1}{\mathfrak{p}}_{e},$
(23) $\displaystyle\dot{{\mathfrak{p}}}_{e}$
$\displaystyle=-\mathbf{e}(\mathbf{x},\mathbf{x}^{*})-\mathbf{K}_{d}\mathbf{M}^{-1}{\mathfrak{p}}_{e}+\mathbf{d}.$

###### Theorem 1

Consider the Hamiltonian system in (14) with desired regulation point
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},0)$ and control law specified in (18) with
parameters $k_{\mathbf{p}}$, $k_{\mathbf{R}}$, $\mathbf{K}_{\mathbf{d}}$.
Assume that the initial state $\mathbf{x}(t_{0})$ lies in the domain ${\cal
A}=\left\\{\mathbf{x}\mid\operatorname{tr}(\mathbf{I}-\mathbf{R}^{*\top}\mathbf{R})\leq\alpha<4,\|{\mathfrak{p}}\|\leq\beta\right\\}$
for some positive constants $\alpha$ and $\beta$. Then, the function:

${\cal V}(\mathbf{x},\mathbf{x}^{*})={\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})+\rho\frac{d}{dt}{\cal
U}_{d}({\mathfrak{q}}_{e})$ (24)

is an ISS-Lyapunov function [63] with respect to $\mathbf{d}$ in (22) and
satisfies:

$\displaystyle k_{1}\lVert\mathbf{z}\rVert^{2}\leq{\cal
V}(\mathbf{x},\mathbf{x}^{*})$ $\displaystyle\leq
k_{2}\lVert\mathbf{z}\rVert^{2},$ (25) $\displaystyle\dot{{\cal
V}}(\mathbf{x},\mathbf{x}^{*})$ $\displaystyle\leq-
k_{3}\lVert\mathbf{z}\rVert^{2}+k_{\gamma}\delta_{\mathbf{d}}^{2},$

where
$\mathbf{z}\coloneqq[\|\mathbf{e}(\mathbf{x},\mathbf{x}^{*})\|\;\|{\mathfrak{p}}_{e}\|]^{\top}\in\mathbb{R}^{2}$,
$k_{\gamma}=\frac{1}{2\lambda_{\min}(\mathbf{K}_{\mathbf{d}})}+\frac{\rho\lambda_{2}^{2}}{2\lambda_{1}}$,
$\lambda_{1}\coloneqq{\color[rgb]{0,0,0}\lambda_{\min}(\mathbf{M}^{-1})}$,
$\lambda_{2}\coloneqq{\color[rgb]{0,0,0}\lambda_{\max}(\mathbf{M}^{-1})}$,
$k_{1}=\frac{1}{2}\lambda_{\min}(\mathbf{Q}_{1})$,
$k_{2}=\frac{1}{2}\lambda_{\max}(\mathbf{Q}_{2})$,
$k_{3}=\frac{1}{2}\lambda_{\min}(\mathbf{Q}_{3})$, and the associated matrices
$\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$, $\mathbf{Q}_{3}$ are defined as:

$\displaystyle\mathbf{Q}_{1}$
$\displaystyle=\begin{bmatrix}\min\left\\{k_{\mathbf{p}}^{-1},k_{\mathbf{R}}^{-1}\right\\}&-\rho\lambda_{2}\\\
-\rho\lambda_{2}&\lambda_{1}\end{bmatrix},$ (26) $\displaystyle\mathbf{Q}_{2}$
$\displaystyle=\begin{bmatrix}\max\left\\{k_{\mathbf{p}}^{-1},\frac{4k_{\mathbf{R}}^{-1}}{4-\alpha}\right\\}&\rho\lambda_{2}\\\
\rho\lambda_{2}&\lambda_{2}\end{bmatrix},\;\mathbf{Q}_{3}=\begin{bmatrix}q_{1}&q_{2}\\\
q_{2}&q_{3}\end{bmatrix},$

where the elements of $\mathbf{Q}_{3}$ are:

$\displaystyle q_{1}$ $\displaystyle=\rho\lambda_{1},$ (27) $\displaystyle
q_{2}$
$\displaystyle=-\rho\left[\lambda_{\max}(\mathbf{M}^{-1}\mathbf{K}_{\mathbf{d}}\mathbf{M}^{-1})+\beta\lambda_{2}^{2}\right],$
$\displaystyle q_{3}$
$\displaystyle=\lambda_{\min}(\mathbf{K}_{\mathbf{d}})\lambda_{1}^{2}-2\rho\lambda_{2}^{2}\max\left\\{k_{\mathbf{p}},k_{\mathbf{R}}\right\\}.$

Denote the sub-level set of ${\cal V}(\mathbf{x},\mathbf{x}^{*})$ with respect
to positive scalar $c$ as: ${\cal S}_{c}\coloneqq\left\\{\mathbf{x}\mid{\cal
V}(\mathbf{x},\mathbf{x}^{*})\leq c\right\\}$. Given constants $c_{1}$,
$c_{2}$ defined as:

$c_{1}\coloneqq\frac{k_{2}k_{\gamma}}{k_{3}}\delta_{\mathbf{d}}^{2},\;c_{2}\coloneqq
k_{1}\min\left\\{k_{\mathbf{R}}^{2}\alpha(4-\alpha)/4,\beta^{2}\right\\},$
(28)

${\cal S}_{c_{2}}\subseteq{\cal A}$ is an estimate of the region of attraction
of the control law in (18). Any state $\mathbf{x}$ starting within ${\cal
S}_{c_{2}}$ will converge exponentially to ${\cal S}_{c_{1}}$ and remain
within it. The position error trajectory $\mathbf{p}_{e}(t)$ is uniformly
ultimately bounded as:

$\lim_{t\rightarrow\infty}\lVert\mathbf{p}_{e}(t)\rVert^{2}\leq\frac{c_{1}}{k_{1}k_{\mathbf{p}}^{2}}=\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}\delta_{\mathbf{d}}^{2}.$
(29)

To ensure that $c_{1}<c_{2}$, the disturbance bound $\delta_{\mathbf{d}}$
should satisfy
$\delta_{\mathbf{d}}<\sqrt{\frac{c_{2}k_{3}}{k_{2}k_{\gamma}}}$.

######  Proof 4.2.

See Appendix 9.

The estimates of the region of attraction and the uniform ultimate bound on
the position error provided by Thm. 1 for the IDA-PBC controller are
conservative because our analysis considers the mass and inertia jointly as a
generalized mass $\mathbf{M}$ and does not differentiate the force and torque
disturbances. Besides considering separate disturbance bounds for the force
and torque inputs, less conservative bounds can be achieved by introducing
disturbance compensation [41].

###  4.3 Safety analysis

Sec. 4.4.2 analyzed the stability and robustness properties of the IDA-PBC
controller for a given regulation point $\mathbf{x}^{*}$. Next, we use the
Lyapunov function ${\cal V}(\mathbf{x},\mathbf{x}^{*})$ in (24) to derive
conditions under which the trajectory of the closed-loop system remains
outside the unsafe set ${\cal O}$. We introduce a barrier function, which
takes the region of attraction ${\cal S}_{c_{2}}$ of the controller and the
invariant set ${\cal S}_{c_{1}}$ associated with the ultimate bound in Thm. 1
as well as the distance $\bar{d}(\mathbf{p}^{*},{\cal O})$ to ${\cal O}$ into
account to quantify the margin to safety violation:

$\Delta
E(\mathbf{x},\mathbf{x}^{*})\coloneqq\min\left\\{c_{2},k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}\right\\}-{\cal
V}(\mathbf{x},\mathbf{x}^{*})\\\ +\max\left\\{c_{1}-{\cal
V}(\mathbf{x},\mathbf{x}^{*}),0\right\\},$ (30)

where $k_{1}$, $k_{\mathbf{p}}$, $c_{1}$, $c_{2}$ are the constants specified
in Thm. 1. If, for a given regulation point $\mathbf{x}^{*}$, the safety
margin $\Delta E(\mathbf{x},\mathbf{x}^{*})$ is positive initially, then any
trajectory of the closed-loop system remains safe as it converges to the
invariant set ${\cal S}_{c_{1}}$.

######  Proposition 4.3.

Consider the system in (14) with regulation point
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},0)$ and control law in (18). Suppose that
the desired position $\mathbf{p}^{*}$ has sufficient clearance from the unsafe
set ${\cal O}$ and the disturbance $\mathbf{d}$ is bounded as follows:

$\bar{d}{\,}^{2}(\mathbf{p}^{*},{\cal
O})\geq\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}\delta_{\mathbf{d}}^{2},\qquad\|\mathbf{d}\|^{2}\leq\delta_{\mathbf{d}}^{2}<\frac{c_{2}k_{3}}{k_{2}k_{\gamma}}.$
(31)

If the initial state $\mathbf{x}(t_{0})=\mathbf{x}_{0}$ satisfies:

$\Delta E(\mathbf{x}_{0},\mathbf{x}^{*})\geq 0,$ (32)

then the position error trajectory is uniformly ultimately bounded as in (29)
and the system remains safe, i.e., $d(\mathbf{p}(t),{\cal O})\geq 0$ for all
$t\geq t_{0}$.

######  Proof 4.4.

By the definition in (30), $\Delta E(\mathbf{x},\mathbf{x}^{*})\geq 0$ implies
that the Lyapunov function ${\cal V}(\mathbf{x},\mathbf{x}^{*})$ satisfies one
of three cases:

  1. 1.

$c_{1}<{\cal V}$, ${\cal
V}\leq\min\left\\{c_{2},k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}\right\\}$,

  2. 2.

$c_{1}\geq{\cal V}$, ${\cal
V}\leq\min\left\\{c_{2},k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}\right\\}$,

  3. 3.

$c_{1}\geq{\cal V}$, ${\cal
V}>\min\left\\{c_{2},k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}\right\\}$.

Case 3) can never happen because (31) implies that $c_{1}\leq
k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}$ and $c_{1}<c_{2}$.

For Case 1), when $c_{1}<{\cal V}\leq c_{2}$, we know from Thm. 1 that
$\dot{{\cal V}}<0$ and every trajectory starting in ${\cal S}_{c_{2}}$
converges exponentially to ${\cal S}_{c_{1}}$. In this case, from (25):

$\displaystyle k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}$ $\displaystyle\geq{\cal
V}(\mathbf{x}(t_{0}),\mathbf{x}^{*})>{\cal V}(\mathbf{x}(t),\mathbf{x}^{*})$
(33) $\displaystyle\geq k_{1}\lVert\mathbf{z}(t)\rVert^{2}\geq
k_{1}k_{\mathbf{p}}^{2}\lVert\mathbf{p}(t)-\mathbf{p}^{*}\rVert^{2}.$

Therefore,
$\lVert\mathbf{p}(t)-\mathbf{p}^{*}\rVert^{2}\leq\bar{d}{\,}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{p}^{*},{\cal O}}\leq d^{2}(\mathbf{p}^{*},{\cal
O})$ and $d(\mathbf{p}(t),{\cal O})\geq 0$ for all
$t\in\left[t_{0},t_{1}\right]$, where $t_{1}$ is the time when the trajectory
enters ${\cal S}_{c_{1}}$, corresponding to Case 2) above.

For Case 2), we have ${\cal V}(\mathbf{x},\mathbf{x}^{*})\leq c_{1}$ since
(31) implies that $c_{1}<c_{2}$. From Thm. 1, ${\cal S}_{c_{1}}$ is forward
invariant and:

$\lVert\mathbf{p}(t)-\mathbf{p}^{*}\rVert^{2}\leq\frac{{\cal
V}(\mathbf{x}(t),\mathbf{x}^{*})}{k_{1}k_{\mathbf{p}}^{2}}\leq\frac{c_{1}}{k_{1}k_{\mathbf{p}}^{2}}=\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}\delta_{\mathbf{d}}^{2}.$
(34)

Hence, (31) implies that $d(\mathbf{p}(t),{\cal O})\geq 0$.

##  5 Safe and Stable Tracking using a Reference Governor

Figure 2: Structure of the reference-governor tracking controller (left). A
governor with state $\mathbf{g}$ adaptively tracks desired path $\mathbf{r}$
and generates a reference point
$\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$ for the closed-loop
Hamiltonian system (right). A local projected goal $\bar{\mathbf{g}}$ (purple
dot) is generated as the farthest intersection between the local safe set
$\mathcal{LS}(\mathbf{x},\mathbf{g})$ (yellow sphere) and the path
$\mathbf{r}$ (blue curve) to guide the governor motion.

In this section, we develop a safe tracking controller by introducing a
reference governor [33] to guide the reference point $\mathbf{x}^{*}$ for the
stabilizing control law ${\boldsymbol{\pi}}(\mathbf{x},\mathbf{x}^{*})$ in
(18) along the desired reference path $\mathbf{r}$ introduced in Problem 1.

A reference governor is a virtual dynamical system whose state $\mathbf{g}(t)$
moves along $\mathbf{r}(\sigma)$ for $\sigma\in[0,1]$. In this paper, the
governor state $\mathbf{g}(t)\in\mathbb{R}^{3}$ specifies a desired position
$\mathbf{p}^{*}(t)$ for the Hamiltonian system. We introduce a lifting
function $\mathbf{x}^{*}(t)={\boldsymbol{\ell}}(\mathbf{g}(t))$ to provide a
desired orientation $\mathbf{R}^{*}(t)$ and specify a reference state
$\mathbf{x}^{*}(t)$ for the Hamiltonian system.

Given $\mathbf{x}^{*}(t)$, we compute the safety margin $\Delta
E(\mathbf{x}(t),\mathbf{x}^{*}(t))$ in (30) and use the leeway amount by which
the margin exceeds $0$ to move the governor state $\mathbf{g}(t)$ along
$\mathbf{r}(\sigma)$. Intuitively, the reference point
$\mathbf{x}^{*}(t)={\boldsymbol{\ell}}(\mathbf{g}(t))$ speeds up when $\Delta
E(\mathbf{x}(t),\mathbf{x}^{*}(t))$ increases, e.g., the distance to obstacles
increases or the system energy function decreases, and vice versa.

Given a point $\mathbf{g}=\mathbf{r}(\sigma)$ on the reference path for some
$\sigma\in[0,1]$, we generate a reference state
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},{\mathfrak{p}}^{*})$ where
${\mathfrak{q}}^{*}=(\mathbf{p}^{*},\mathbf{R}^{*})=(\mathbf{g},\mathbf{I})$
and ${\mathfrak{p}}^{*}=0$. The governor state $\mathbf{g}$ represents the
desired position $\mathbf{p}^{*}$ on the path. For simplicity, we set the
desired rotation matrix $\mathbf{R}^{*}=\mathbf{I}$. If, in addition to
$\mathbf{r}$, a desired yaw angle reference is provided, one can generate
$\mathbf{R}^{*}$ using the method described in [64] to achieve better
orientation tracking. We define a lifting function
${\boldsymbol{\ell}}:\mathbb{R}^{3}\mapsto T^{*}SE(3)$ that generates a
reference state $\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$ from the
governor state $\mathbf{g}$:

${\boldsymbol{\ell}}(\mathbf{g})\coloneqq\begin{bmatrix}\mathbf{g}^{\top}&\mathbf{e}_{1}^{\top}&\mathbf{e}_{2}^{\top}&\mathbf{e}_{3}^{\top}&\bf
0^{\top}&\bf 0^{\top}\end{bmatrix}^{\top},$ (35)

where $\mathbf{e}_{1}$, $\mathbf{e}_{2}$, $\mathbf{e}_{3}$ are the rows of the
identity matrix. Given the reference state
$\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$, we compute the safety margin
$\Delta E(\mathbf{x},\mathbf{x}^{*})$ in (30) and describe how to update the
governor state to ensure that safety is preserved.

We update the governor state $\mathbf{g}(t)=\mathbf{r}(\sigma(t))$ along the
path by regulating the parameter $\sigma$:

$\mathbf{g}(t)=\mathbf{r}(\sigma(t)),\quad\dot{\sigma}(t)=-k_{g}(\sigma(t)-\sigma^{*}(t)),$
(36)

where $k_{g}>0$ is a control gain and $\sigma^{*}(t)\in\left[0,1\right]$ is a
desired time-varying parameter, which we construct using the safety margin
$\Delta E(\mathbf{x},\mathbf{x}^{*})$. We require $\sigma^{*}(t)$ to satisfy
two conditions: 1) always stay ahead of the current $\sigma(t)$:
$\sigma^{*}(t)\geq\sigma(t)$, $\forall t\geq t_{0}$, and 2) have distance
$\lVert\sigma^{*}(t)-\sigma(t)\rVert$ proportional to $\Delta
E(\mathbf{x}(t),\mathbf{x}^{*}(t))$. The first condition guarantees that the
governor state $\mathbf{g}(t)$ moves forward along the path towards the goal
$\mathbf{r}(1)$. The second condition allows the safety margin $\Delta E$ to
adaptively regulate the governor state $\mathbf{g}(t)$ in order to ensure
safety for the Hamiltonian system. To construct the desired path parameter
$\sigma^{*}$, we define a _local safe zone_ as a ball around the governor
state $\mathbf{g}$ with radius $\Delta E(\mathbf{x},\mathbf{x}^{*})$ based on
the state $\mathbf{x}$ and the reference state
$\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$.

######  Definition 5.5.

A _local safe zone_ is a subset of $\mathbb{R}^{3}$ that depends on the system
state $\mathbf{x}$ and the governor state $\mathbf{g}$:

$\mathcal{LS}(\mathbf{x},\mathbf{g})\\!\coloneqq\\!\left\\{\mathbf{q}\in\mathbb{R}^{3}\\!\mid\\!\lVert\mathbf{q}-\mathbf{g}\rVert^{2}\leq\Delta
E\\!{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{x},{\boldsymbol{\ell}}(\mathbf{g})}\right\\}\\!,$
(37)

where ${\boldsymbol{\ell}}$ is the lifting function in (35) and $\Delta E$ is
the safety margin in (30).

We determine $\sigma^{*}$ as the farthest intersection between the local safe
zone $\mathcal{LS}(\mathbf{x},\mathbf{g})$ and the path $\mathbf{r}$ by
solving the scalar optimization problem in Def. 5.6.

######  Definition 5.6.

A _local projected goal_ for system-governor state $(\mathbf{x},\mathbf{g})$
is a point $\bar{\mathbf{g}}\in\mathcal{LS}(\mathbf{x},\mathbf{g})$ that is
farthest along the path $\mathbf{r}$:

$\bar{\mathbf{g}}=\mathbf{r}(\sigma^{*}),\;\;\sigma^{*}=\operatorname*{argmax}_{\sigma\in[0,1]}\left\\{\sigma\mid\mathbf{r}(\sigma)\in\mathcal{LS}(\mathbf{x},\mathbf{g})\right\\}.$
(38)

The construction of the local projected goal $\bar{\mathbf{g}}$ is shown in
Fig. 2 (right), showing a reference path $\mathbf{r}$, the local safe zone
$\mathcal{LS}(\mathbf{x},\mathbf{g})$ and the local projected goal
$\bar{\mathbf{g}}$. This constructing of $\sigma^{*}$ and $\bar{\mathbf{g}}$
completes the governor update law (36).

Our safe tracking controller consists of the reference governor system in
(36), adaptively updating the reference point
$\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$ via the lifting function in
(35), and the passivity-based controller in (18) that drives the Hamiltonian
system towards $\mathbf{x}^{*}$. The stability, safety, and robustness of the
proposed tracking controller are analyzed in Thm. 5.7.

######  Theorem 5.7.

Suppose that the desired path $\mathbf{r}(\sigma)$ has sufficient clearance
from the unsafe set ${\cal O}$ and the disturbance $\mathbf{d}$ is bounded as:

$\min_{\sigma\in\left[0,1\right]}\bar{d}{\,}^{2}(\mathbf{r}(\sigma),{\cal
O})\geq\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}\delta_{\mathbf{d}}^{2},\quad\|\mathbf{d}\|^{2}\leq\delta_{\mathbf{d}}^{2}<\frac{c_{2}k_{3}}{k_{2}k_{\gamma}}.$

Consider the Hamiltonian system in (14), the governor system in (36) with
$\sigma^{*}$ constructed in Def. 5.6 and the control law
$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x},{\boldsymbol{\ell}}(\mathbf{g}))$ in
(18). Suppose that the initial state $(\mathbf{x}_{0},\mathbf{g}_{0})$
satisfies:

$\Delta E{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{x}_{0},{\boldsymbol{\ell}}(\mathbf{g}_{0})}>0,\quad\mathbf{g}_{0}=\mathbf{r}(0)=\mathbf{p}(t_{0}),$
(39)

where $\Delta E(\mathbf{x},\mathbf{x}^{*})$ is the safety margin in (30). The
position $\mathbf{p}(t)$ of (14) converges to a ball of radius
$\sqrt{\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}}\delta_{\mathbf{d}}$
around $\mathbf{r}(1)$ and remains safe, i.e. $\mathbf{p}(t)\in{\cal F}$, for
all $t\geq t_{0}$

######  Proof 5.8.

To simplify notation, let $\Delta E(t)=\Delta E{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{x}(t),{\boldsymbol{\ell}}(\mathbf{g}(t))}$.
Initially,
$\mathbf{g}_{0}=\mathbf{p}(t_{0})=\mathbf{r}(0)\in\mathcal{LS}(\mathbf{x}_{0},\mathbf{g}_{0})$
and $\Delta E(t_{0})>0$. The local projected goal $\bar{\mathbf{g}}$ and the
associated $\sigma^{*}$ are well defined in Def. 5.6. By the governor update
law (36), the path parameter $\sigma$ increases, the governor state
$\mathbf{g}(\sigma)$ moves along $\mathbf{r}$ towards the goal
$\mathbf{r}(1)$. The desired state
$\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$ is updated via the lifting
function (35). As $\mathbf{g}$ tracks $\bar{\mathbf{g}}$ on the path
$\mathbf{r}$ via the path parameter update in (36), the system state
$\mathbf{x}$ tracks $\mathbf{x}^{*}={\boldsymbol{\ell}}(\mathbf{g})$. During
this process, the safety margin $\Delta E(t)$ fluctuates and regulates the
rate of change of $\sigma$.

Since $\sigma^{*}(t)$ is bounded in (38), $\sigma(t)$ is updated continuously
[65] in (36), leading to a continuous governor state $\mathbf{g}(t)$. By
construction, the lifting function ${\boldsymbol{\ell}}(\mathbf{g})$ is
continuous in $\mathbf{g}$. Therefore, the reference point
$\mathbf{x}^{*}(t)={\boldsymbol{\ell}}(\mathbf{g}(t))$ is continuous in time,
leading to a continuous Lyapunov function ${\cal
V}(\mathbf{x},\mathbf{x}^{*})$ and a continuous safety margin $\Delta E(t)$.
As a result, the safety margin $\Delta E(t)$ cannot become negative without
crossing $0$ from above at some time $T_{0}$. As $\Delta E(t)\downarrow 0$,
the local safe zone shrinks to a point, i.e.,
$\mathcal{LS}(\mathbf{x},\mathbf{g})\downarrow\left\\{\mathbf{g}\right\\}$.
This immediately stops the the governor because
$\bar{\mathbf{g}}=\mathbf{g}(T_{0})=\mathbf{r}(\sigma(T_{0}))$ and
$\dot{\sigma}(T_{0})=0$.

As a result, Proposition 4.3 states that $\mathbf{x}(t)$ stays within the
invariant set ${\cal S}_{c_{2}}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{x}^{*}(T_{0})}$ for $t\geq T_{0}$ and converges
to ${\cal S}_{c_{1}}{\rm Phys.~{}Rev.~{}Lett.}{\mathbf{x}^{*}(T_{0})}$ without
leaving ${\cal F}$. Eq. (30) shows that $\Delta E(t)=0$ implies
$c_{1}\leq{\cal V}(t)\leq c_{2}$. By Thm. 1, as $\mathbf{x}(t)$ approaches
$\mathbf{x}^{*}(T_{0})$, we have $\dot{{\cal V}}(T_{0})<0$, i.e., the Lyapunov
function ${\cal V}$ is decreasing. There exists $h>0$ such that $\Delta
E(T_{0}+h)$ becomes strictly positive. Hence, the governor is able to move
again towards a new $\bar{\mathbf{g}}$ generated by the positive $\Delta
E(T_{0}+h)$. This process continues until the governor state $\mathbf{g}(t)$
converges to $\mathbf{r}(1)$, the closed-loop system converges to the region
${\cal S}_{c_{1}}{\rm
Phys.~{}Rev.~{}Lett.}{{\boldsymbol{\ell}}(\mathbf{r}(1))}$ and the position
$\mathbf{p}(t)$ satisfies the uniform ultimate bound in (29) around
$\mathbf{r}(1)$.

Note that while our control design does not account for state estimation
errors, e.g. from an odometry algorithm with a sensor setup (e.g. stereo
camera, LiDAR, or visual-inertial), we can conservatively handle the errors by
reducing the obstacle distance $\bar{d}$ in the safety margin specification in
(31).

##  6 Application to Hamiltonian Dynamics in $\mathbb{R}^{n}$

In this section, we show that our control design can be easily modified and
applied to a Hamiltonian system with configuration ${\mathfrak{q}}$ in
$\mathbb{R}^{n}$ and dynamics:

$\begin{bmatrix}\dot{{\mathfrak{q}}}\\\ \dot{{\mathfrak{p}}}\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{0}&\mathbf{I}_{n}\\\
-\mathbf{I}_{n}&\mathbf{0}\end{bmatrix}\begin{bmatrix}\nabla_{{\mathfrak{q}}}{\cal
H}({\mathfrak{q}},{\mathfrak{p}})\\\ \nabla_{{\mathfrak{p}}}{\cal
H}({\mathfrak{q}},{\mathfrak{p}})\end{bmatrix}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{B}({\mathfrak{q}})\end{bmatrix}\mathbf{u}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{d}_{1}\end{bmatrix}$ (40)

where the Hamiltonian ${\cal H}({\mathfrak{q}},{\mathfrak{p}})$ is defined as:

$\mathcal{H}({\mathfrak{q}},{\mathfrak{p}})=\frac{1}{2}{\mathfrak{p}}^{\top}\mathbf{M}^{-1}({\mathfrak{q}}){\mathfrak{p}}+{\cal
U}({\mathfrak{q}}).$ (41)

Given a desired regulation point
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},{\mathfrak{p}}^{*})$ with momentum
${\mathfrak{p}}^{*}=0$, define the error state
$\mathbf{x}_{e}=({\mathfrak{q}}_{e},{\mathfrak{p}}_{e})$ as:

${\mathfrak{q}}_{e}={\mathfrak{q}}-{\mathfrak{q}}^{*},\qquad{\mathfrak{p}}_{e}={\mathfrak{p}}-{\mathfrak{p}}^{*}.$
(42)

A desired Hamiltonian, minimized at $\mathbf{x}=\mathbf{x}^{*}$, is:

$\mathcal{H}_{d}(\mathbf{x},\mathbf{x}^{*})=\frac{1}{2}{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}({\mathfrak{q}}_{e}){\mathfrak{p}}_{e}+\frac{k_{\mathbf{p}}}{2}\lVert{\mathfrak{q}}_{e}\rVert^{2}.$
(43)

The IDA-PBC controller:

$\mathbf{u}={\boldsymbol{\pi}}(\mathbf{x},\mathbf{x}^{*})=\mathbf{B}^{\dagger}({\mathfrak{q}})\mathbf{b}(\mathbf{x},\mathbf{x}^{*})$
(44)

with
$\displaystyle{\mathbf{b}(\mathbf{x},\mathbf{x}^{*})=\nabla_{{\mathfrak{q}}}{\cal
H}(\mathbf{x})-\nabla_{{\mathfrak{q}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})-\mathbf{K}_{\mathbf{d}}\nabla_{{\mathfrak{p}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})}$ achieves the closed-loop dynamics:

$\begin{bmatrix}\dot{\mathbf{{\mathfrak{q}}}}_{e}\\\
\dot{\mathbf{{\mathfrak{p}}}}_{e}\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{0}&\mathbf{I}_{n}\\\
-\mathbf{I}_{n}&-\mathbf{K}_{d}\end{bmatrix}\begin{bmatrix}\nabla_{{\mathfrak{q}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})\\\ \nabla_{{\mathfrak{p}}_{e}}{\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})\end{bmatrix}+\begin{bmatrix}\mathbf{0}\\\
\mathbf{d}\end{bmatrix},$ (45)

where $\mathbf{d}=\mathbf{d}_{1}+\mathbf{d}_{2}$ as in (22) and
$\mathbf{d}_{2}$ is as in (21).

Figure 3: $SE(3)$ Hamiltonian neural ODE network (left to right): (a)
simulated hexarotor for evaluation, (b) training loss, (c) learned inverse
inertia $\mathbf{J}_{\boldsymbol{\theta}}({\mathfrak{q}})^{-1}$, and (d)
learned input matrix $\mathbf{B}_{\boldsymbol{\theta}}(\mathfrak{q})$ along a
test trajectory, evaluated on the simulated hexarotor.

######  Theorem 6.9.

Consider the Hamiltonian system in (40) with desired regulation point
$\mathbf{x}^{*}=({\mathfrak{q}}^{*},\mathbf{0})$ and control law in (44) with
parameters $k_{\mathbf{p}}$, $\mathbf{K}_{\mathbf{d}}$. Under Assumptions 1 &
2, the function:

${\cal V}(\mathbf{x},\mathbf{x}^{*})={\cal
H}_{d}(\mathbf{x},\mathbf{x}^{*})+\rho\frac{d}{dt}{\cal
U}_{d}({\mathfrak{q}}_{e})$ (46)

with ${\cal
U}_{d}({\mathfrak{q}}_{e})=\frac{k_{\mathbf{p}}}{2}\|{\mathfrak{q}}_{e}\|^{2}$
is an ISS-Lyapunov function [63] with respect to $\mathbf{d}$ and satisfies:

$\displaystyle k_{1}\lVert\mathbf{z}\rVert^{2}\leq{\cal
V}(\mathbf{x},\mathbf{x}^{*})$ $\displaystyle\leq
k_{2}\lVert\mathbf{z}\rVert^{2},$ (47) $\displaystyle\dot{{\cal
V}}(\mathbf{x},\mathbf{x}^{*})$ $\displaystyle\leq-
k_{3}\lVert\mathbf{z}\rVert^{2}+k_{\gamma}\delta_{\mathbf{d}}^{2},$

where
$\mathbf{z}\coloneqq[k_{\mathbf{p}}\|{\mathfrak{q}}_{e}\|\;\|{\mathfrak{p}}_{e}\|]^{\top}\in\mathbb{R}^{2}$,
$k_{\gamma}=\frac{1}{2\lambda_{\min}(\mathbf{K}_{\mathbf{d}})}+\frac{\rho\lambda_{2}^{2}}{2\lambda_{1}}$,
$\lambda_{1}\coloneqq\lambda_{\min}(\mathbf{M}^{-1})$,
$\lambda_{2}\coloneqq\lambda_{\max}(\mathbf{M}^{-1})$,
$k_{1}=\frac{1}{2}\lambda_{\min}(\mathbf{Q}_{1})$,
$k_{2}=\frac{1}{2}\lambda_{\max}(\mathbf{Q}_{2})$,
$k_{3}=\frac{1}{2}\lambda_{\min}(\mathbf{Q}_{3})$, and the associated matrices
$\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$, $\mathbf{Q}_{3}$ are defined as:

$\displaystyle\mathbf{Q}_{1}$
$\displaystyle=\begin{bmatrix}k_{\mathbf{p}}^{-1},&-\rho\lambda_{2}\\\
-\rho\lambda_{2}&\lambda_{1}\end{bmatrix},\quad\mathbf{Q}_{2}=\begin{bmatrix}k_{\mathbf{p}}^{-1},&\rho\lambda_{2}\\\
\rho\lambda_{2}&\lambda_{2}\end{bmatrix},$ (48) $\displaystyle\mathbf{Q}_{3}$
$\displaystyle=\begin{bmatrix}\rho\lambda_{1}&-\rho\gamma_{d}\lambda_{2}^{2}\\\
-\rho\gamma_{d}\lambda_{2}^{2}&\gamma_{d}\lambda_{1}^{2}-2\rho\lambda_{2}^{2}k_{\mathbf{p}}\end{bmatrix}.\quad$

Any initial state $\mathbf{x}$ converges exponentially to ${\cal
S}_{c_{1}}=\left\\{\mathbf{x}|{\cal V}(\mathbf{x},\mathbf{x}^{*})\leq
c_{1}\right\\}$ with
$c_{1}\coloneqq\frac{k_{2}k_{\gamma}}{k_{3}}\delta_{\mathbf{d}}^{2}$ and
remains within. The error trajectory ${\mathfrak{q}}_{e}(t)$ is uniformly
ultimately bounded:

$\lim_{t\rightarrow\infty}\lVert{\mathfrak{q}}_{e}(t)\rVert^{2}\leq\frac{c_{1}}{k_{1}k_{\mathbf{p}}^{2}}=\frac{k_{2}k_{\gamma}}{k_{1}k_{3}k_{\mathbf{p}}^{2}}\delta_{\mathbf{d}}^{2}.$
(49)

The proof of Thm. 6.9 follows the same steps as that of Thm. 1, and is omitted
due to space limitations. In contrast to Thm. 1, the result in Thm. 6.9 for
$\mathbb{R}^{n}$ holds globally, i.e., the region of attraction is ${\cal
A}=\mathbb{R}^{n}\times\mathbb{R}^{n}$. Thus, the disturbance magnitude bound
$\delta_{\mathbf{d}}$ can be arbitrarily large.

The safety analysis in Sec. 4.4.3 can be modified with a new safety margin:

$\Delta E(\mathbf{x},\mathbf{x}^{*})\coloneqq
k_{1}k_{\mathbf{p}}^{2}\bar{d}{\,}^{2}\\!{\rm
Phys.~{}Rev.~{}Lett.}{{\mathfrak{q}}^{*},{\cal O}}-{\cal
V}(\mathbf{x},\mathbf{x}^{*})\\\ +\max\left\\{c_{1}-{\cal
V}(\mathbf{x},\mathbf{x}^{*}),0\right\\},$ (50)

as Thm. 6.9 holds globally. The reference governor lifting function can be
chosen as $\ell(\mathbf{g})=[\mathbf{g}^{\top}\;\bf 0^{\top}]^{\top}$. The
governor state update remains the same as in (36). The robustness analysis
extends the safe tracking results in [66] and [36].

Figure 4: Comparison of position prediction errors (left) between the learned
Hamiltonian dynamics and GP model, tracking errors (middle) and distance to
obstacles (right) between our safe tracking controller and GP-MPC.

Figure 5: Path tracking with our approach (left) and GP-MPC [7] (right).

##  7 Evaluation

We evaluate our robust and safe tracking controller using simulated hexarotor
and quadrotor robots in 2D and 3D environments with ground-truth mass $m=6.77$
kg, and inertia matrix $\mathbf{J}=\text{diag}([1.05,1.05,2.05])$
kg$\,\cdot\,$m2 , inspired by the solar-powered UAV in [67]. The robot’s
ground-truth dynamics satisfy Hamilton’s equations (3) with generalized mass
$\mathbf{M}(\mathbf{{\mathfrak{q}}})=\operatorname*{diag}(m\mathbf{I},\mathbf{J})$,
potential energy ${\cal
U}({\mathfrak{q}})=mg\begin{bmatrix}0&0&1\end{bmatrix}\mathbf{p}$, where
$\mathbf{p}$ is the position and $g\approx 9.8ms^{-2}$ is the gravitational
acceleration. The input matrices for the hexarotor and the quadrotor are
$\mathbf{B}({\mathfrak{q}})=\mathbf{I}$ and
$\mathbf{B}({\mathfrak{q}})=\begin{bmatrix}\bf 0_{4\times
2}&\mathbf{I}_{4\times 4}\end{bmatrix}^{\top}$, respectively. The control
input $\mathbf{u}$ of the hexarotor includes a 3D force and a 3D torque while
that of the quadrotor includes a scalar force and a 3D torque.

For all experiments, the following control gains are used for our controller
in Sec. 4.4.1: $k_{\mathbf{p}}=20$, $k_{\mathbf{R}}=50$,
$\mathbf{K}_{d}=15\mathbf{I}$ in (16). The parameters shown in Thm. 1 are:
$\alpha=2$, $\beta=20$, $c_{1}=2.2050$, $c_{2}=8.8200$, $\rho=3.5822\times
10^{-5}$. The control gain for the governor in (36) is $k_{g}=0.5$. The
control loop frequency for all experiments is at $120$ Hz.

While, our evaluation focuses on rotorcraft aerial robots, the methodology for
system identification and control synthesis proposed in this paper is general.
The exact same approach is applied to hexarotor, quadrotor and other ground
and marine vehicles. This is in contrast with other system identification and
control synthesis methods, which require knowledge of the dynamics structure,
careful experiment design, and domain expertise for the particular system.

###  7.1 Evaluation of $SE(3)$ Hamiltonian dynamics learning

We consider a simulated hexarotor unmanned aerial vehicle (UAV) (Fig. 3) with
fixed-tilt rotors pointing in different directions [68] and a simulated
quadrotor UAV. Since the mass $m$ of the UAVs can be easily measured, we
assume the mass $m$ is known, leading to a known potential energy ${\cal
U}({\mathfrak{q}})=mg\begin{bmatrix}0&0&1\end{bmatrix}\mathbf{p}$. We
approximate the inverse generalized mass matrix by
$\mathbf{M}^{-1}_{\boldsymbol{\theta}}(\mathbf{{\mathfrak{q}}})=\operatorname*{diag}(m^{-1}\mathbf{I},\mathbf{J}_{{\boldsymbol{\theta}}}^{-1}(\mathbf{{\mathfrak{q}}}))$
and learn $\mathbf{J}_{{\boldsymbol{\theta}}}(\mathbf{{\mathfrak{q}}})^{-1}$
and $\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$ from data.

We mimic manual flights in an area free of obstacles using a PID controller
and drive the UAVs from a random initial pose to $18$ desired poses,
generating $18$ $1$-second trajectories. We shift the trajectories to start
from the origin and create a dataset
$\mathcal{D}=\\{t_{0:N}^{(i)},\mathbf{{\mathfrak{q}}}_{0:N}^{(i)},{\boldsymbol{\zeta}}_{0:N}^{(i)},\mathbf{u}^{(i)}_{0:N-1})\\}_{i=1}^{D}$
with $N=24$ and $D=18$. The Hamiltonian-based neural ODE network is trained
with the dataset ${\cal D}$, as described in Sec. 3, for 5000 iterations and
learning rate $10^{-4}$. For the hexarotor, Fig. 3(c) shows the loss function
during training. Note that if we scale
$\mathbf{M}_{\boldsymbol{\theta}}({\mathfrak{q}})$ and the input matrix
$\mathbf{B}({\mathfrak{q}})$ by a constant $\gamma$, the dynamics of
$({\mathfrak{q}},{\boldsymbol{\zeta}})$ in (3) and (5) does not change. Fig.
3(d) and 3(e) plot the scaled version of the learned inverse mass
$\mathbf{J}_{\boldsymbol{\theta}}({\mathfrak{q}})^{-1}$ and the input matrix
$\mathbf{B}_{\boldsymbol{\theta}}({\mathfrak{q}})$, converging to the constant
ground truth values. We achieve similar results for the quadrotor using the
same training process.

###  7.2 Evaluation of robust safe tracking control of a learned 2D hexarotor
Hamiltonian model

Next, we compare our approach with a GP-MPC technique [7] using a simulated 2D
fully-actuated hexarotor UAV, moving on the xz-plane with position
$\mathbf{p}=\begin{bmatrix}x,0,z\end{bmatrix}$ and orientation
$\mathbf{R}=\mathbf{R}_{\psi}$ determined by the pitch angle $\psi$. The
control input is a 3D wrench, including a 2D force and a 1D torque.

As we only consider the pitch angle $\psi$, we are interested in the inertia
value $J^{yy}$ and ignore $J^{xx}$ and $J^{zz}$. We assume that the
generalized mass $m$ and $J^{yy}$ are unknown for the 2D hexarotor and
approximated by $m_{\boldsymbol{\theta}}$ and $J^{yy}_{\boldsymbol{\theta}}$,
respectively. The input gain $\mathbf{B}(\mathbf{{\mathfrak{q}}})$ is assumed
known.

Let $m_{0}=1.5m$ and $J^{yy}_{0}=1.5J^{yy}$ be initial guesses of the mass $m$
and the inertia $J^{yy}$. We model the approximated mass inverse
$m^{-1}_{\boldsymbol{\theta}}$ and inertia inverse
${J^{yy}_{\boldsymbol{\theta}}}^{-1}$ as:

$\displaystyle{m^{-1}_{\boldsymbol{\theta}}={\rm
Phys.~{}Rev.~{}Lett.}{\sqrt{m_{0}^{-1}}+L_{1}({\mathfrak{q}};{\boldsymbol{\theta}})}^{2},{J^{yy}_{\boldsymbol{\theta}}}^{-1}={\rm
Phys.~{}Rev.~{}Lett.}{\sqrt{{J^{yy}_{0}}^{-1}}+L_{2}({\mathfrak{q}};{\boldsymbol{\theta}})}^{2},}$

where $L_{1}({\mathfrak{q}};{\boldsymbol{\theta}})$ and
$L_{2}({\mathfrak{q}};{\boldsymbol{\theta}})$ are two neural networks,
representing the residual mass inverse and inertia inverse to be learned. In
GP-MPC [7], the dynamics (3) are split into a prior nominal model with the
prior mass $m_{0}$ and inertia $J^{yy}_{0}$, and residual dynamics, modeled by
a GP regression model.

To collect training data, we place the simulated hexarotor at an initial
location $(x,z)=(-1,0)$ and apply random control inputs to obtain
$\mathcal{D}=\\{t_{0:1}^{(i)},\mathbf{{\mathfrak{q}}}_{0:1}^{(i)},{\boldsymbol{\zeta}}_{0:1}^{(i)},\mathbf{u}^{(i)}_{0}\\}_{i=1}^{150}$.
Our Hamiltonian neural ODE network is trained with the dataset ${\cal D}$, as
described in Sec. 3. For GP-MPC, the same dataset $\mathcal{D}$ is used to
train a GP regression model of the residual dynamics as described in [7] and
implemented in [38].

We assume there are two walls in the environments, generating two safety
constraints on the robot position: $-x+z<1.1,\;0.8x+z<0.4$. The task is to
track a predefined piecewise linear path $\mathbf{r}$, shown in Fig. 5, while
safely avoiding collision with the walls. We adapt the GP-MPC implementation
by [38] for the 2D hexarotor and enforce the safety constraints
probabilistically with $95\%$ confidence interval using the GP model
uncertainty. To propagate the model uncertainty through a horizon of $10$ time
steps, we linearize the dynamics model around the hovering state and propagate
the state mean and covariance using the mean equivalence technique [7, 38]
with a time step of $1/120$ s. Meanwhile, our learned Hamiltonian neural ODE
model is used with the safe tracking controller described in Sec. 5 to perform
the task and enforce safety constraints.

Fig. 4 compares the prediction errors of our learned neural ODE network and
the GP model. We collect the robot states and control inputs, generated by our
controller while tracking the path, and predict the next state. Fig. 4 (left)
plots the prediction error over time, showing that we achieve better
prediction than the trained GP model. This reflects the difference between our
model, which encodes the Hamiltonian structure and translation equivariance in
the network architecture, and the GP model, which incurs higher model
uncertainty in locations far from the data points.

Fig. 4 and 5 show tracking performance of our approach and GP-MPC. We compare
the tracking error of both methods, calculated as the distance from the robot
position to the reference point, specified by the governor in our approach and
by time parameterization of the path in GP-MPC:
$\mathbf{p}^{*}(t)=\mathbf{r}(\min(t,10)/10)$, i.e., the GP-MPC method
finishes the task in about $10$ seconds, similar to the tracking time of our
approach. Our controller is able to track the path more accurately than GP-
MPC, illustrated qualitatively in Fig. 5 and quantitatively in Fig. 4
(middle). This can be explained by the higher predictions errors shown in Fig.
4 (left), which grow quickly after multiple time steps due to uncertainty
propagation. Both our safe tracking controller with learned Hamiltonian
dynamics and the GP-MPC safe controller keep the hexarotor in the safe region,
i.e., the distance to the obstacles is always positive in Fig. 4 (right).

Figure 6: Safe navigation of a hexarotor system using learned model in a
warehouse (left). The hexarotor (red body) navigates from a start (red star)
to a goal location (green star) while avoiding obstacles. The obstacles are
sensed by a simulated LiDAR sensor. The reference path, the robot path are
shown in blue and green, respectively. Local safe set is shown in yellow
sphere. The right plots show the dynamic safety margin $\Delta E$, the
Lyapunov function ${\cal V}$, and the distance to the obstacles
$\bar{d}(\mathbf{p}(t),{\cal O})$, indicating that the safety constraints are
never violated. Figure 7: Tracking error of a hexarotor system (top to
bottom): position, velocity, angle and angular velocity errors. Figure 8:
Tracking controller performance for hexarotor in warehouse simulation with the
ground truth model subject to a disturbance $\mathbf{d}$ with different
magnitudes: the average position tracking error (top) and the minimum distance
to obstacle (bottom).

###  7.3 Evaluation of robust safe tracking control of a learned 3D fully-
actuated hexarotor Hamiltonian model

This section evaluates our Hamiltonian dynamics learning and safe tracking
control techniques using a simulated hexarotor UAV in a 3D environment. The
task is to navigate from a start position to a goal in a cluttered warehouse
environment without colliding with the obstacles ${\cal O}$. The same control
gains are used for this 3D navigation task as in the previous section. A
simulated LiDAR scanner provides point cloud measurements ${\cal P}(t)$ of the
surface of the unsafe set ${\cal O}$, depending on the system pose at time
$t$, with a maximum sensing range of $d_{max}=30$ m. The distance from the
governor $\mathbf{g}(t)$ to the unsafe set ${\cal O}$ is approximated via
$\bar{d}(\mathbf{g}(t);{\cal O})\approx\min_{\mathbf{y}\in{\cal
P}(t)}\lVert\mathbf{g}(t)-\mathbf{y}\rVert$. The reference path $\mathbf{r}$
is pre-computed using an A* planner and tracked in $\sim 80$ s.

Fig. 6 shows the behavior of the closed-loop hexarotor system in the warehouse
environment. The safety margin $\Delta E(\mathbf{x},\mathbf{x}^{*})$
fluctuates during the tracking process but, as can be seen in Fig. 6, it never
becomes negative. The augmented system $(\mathbf{x},\mathbf{g})$ is controlled
adaptively, slowing down when the dynamic safety margin decreases (e.g., when
the hexarotor is close to an obstacle or has large Lyapunov value ${\cal V}$)
and speeding up otherwise (e.g., when the robot is far away from the obstacles
or has small total energy ${\cal V}$). The simulations show that our control
policy successfully drives the system from the start to the end of the
reference path while avoiding sensed obstacles online, i.e.,
$d(\mathbf{p},{\cal O})$ remains positive throughout the tracking task. Fig. 7
plots the tracking errors between the robot state $\mathbf{x}$ and the
reference state $\mathbf{x}^{*}$ generated by the governor, showing that our
controller tracks the path well. The tracking errors for the Euler angles and
angular velocity, are close to $0$. The position and linear velocity errors in
the $x$ and $z$ directions are close to zero as well while the errors in $y$
direction fluctuates around $-0.5$ m and $0.8$ m/s, respectively, and
converges to $0$ at the end. This is expected as the robot stays behind the
reference point, mostly in $y$ direction, and converges to the end of the
path.

To evaluate the robustness of our controller, we repeat the warehouse
experiment using the ground-truth dynamics, subject to a artificially
generated disturbances $\mathbf{d}\in\mathbb{R}^{6}$ with different upper
bounds $\delta_{\mathbf{d}}$. Each component of the disturbance
$\mathbf{d}\in\mathbb{R}^{6}$ is uniformly generated in
$\left[-0.5\delta_{\mathbf{d}},0.5\delta_{\mathbf{d}}\right]$. If
$\lVert\mathbf{d}\rVert>\delta_{\mathbf{d}}$, we normalize the disturbance as
$\delta_{\mathbf{d}}\mathbf{d}/\lVert\mathbf{d}\rVert$. Our robust tracking
controller successfully finishes the tracking task across a wide range of
$\delta_{\mathbf{d}}$: $\left[0.001,0.01,0.1,1,10,20,30\right]$. Larger
$\delta_{\mathbf{d}}$ are not reported due to violation of the positiveness
requirement on $\Delta E$. Fig. 8 shows the average position errors and the
minimum distance to obstacle during the tracking task versus the disturbance
upper bound $\delta_{\mathbf{d}}$. The average position tracking errors remain
similar against $\delta_{\mathbf{d}}$. The minimum distance to obstacle
$d(\mathbf{p},{\cal O})$ is always positive, illustrating the safety
guarantees of our controller. This number starts decreasing when
$\delta_{\mathbf{d}}>1$ as larger disturbances can suddenly move the robot
towards the obstacles.

###  7.4 Evaluation of robust safe tracking control of a learned 3D
underactuated quadrotor Hamiltonian model

In this section, we repeat the task of safely navigating from a start position
to a goal in the same cluttered warehouse environment in Sec. 7.7.3 with a
quadrotor, whose model is learned from data as described in Sec. 7.7.1. As
mentioned in Sec. 4, the control input in (18) would not introduce additional
disturbance $\mathbf{d}_{2}$ when the matching condition (20) is satisfied.
For quadrotor, a maximal-rank left annihilator of the ground-truth
$\mathbf{B}({\mathfrak{q}})=\begin{bmatrix}\bf 0_{4\times
2}&\mathbf{I}_{4\times 4}\end{bmatrix}^{\top}$ is
$\mathbf{B}^{\dagger}({\mathfrak{q}})=\begin{bmatrix}\mathbf{I}_{2\times
2}&\bf 0_{2\times 4}\end{bmatrix}$. The matching condition (20) is satisfied
if and only if the first two elements of
$\mathbf{b}(\mathbf{x},\mathbf{x}^{*})=\begin{bmatrix}\mathbf{b}_{\mathbf{v}}^{\top}&\mathbf{b}_{\boldsymbol{\omega}}^{\top}\end{bmatrix}^{\top},\mathbf{b}_{\mathbf{v}}\in\mathbb{R}^{3},\mathbf{b}_{\boldsymbol{\omega}}\in\mathbb{R}^{3}$
in (19) equal to $0$, i.e. the force component $\mathbf{b}_{\mathbf{v}}$
coincides with the $z$-axis of the body frame. As guaranteeing this condition
is hard, we instead use the force component in the world frame
$\mathbf{R}\mathbf{b}_{\mathbf{v}}$ and a desired yaw angle $\psi^{*}$ to
determine the desired rotation matrix, similar to [64]. The vector
$\mathbf{R}\mathbf{b}_{\mathbf{v}}$ is set as the $z$-axis of the desired
frame, i.e., the third column $\mathbf{b}_{3}^{*}$ of the rotation matrix
$\mathbf{R}^{*}$, to minimize the disturbance $\mathbf{d}_{2}$ in (21) from
the matching condition. We calculate the second column $\mathbf{b}^{*}_{2}$ by
projecting the second column of the yaw’s rotation matrix
$\mathbf{b}_{2}^{\psi}=[-\cos{\psi},\sin{\psi},0]$ onto the plane
perpendicular to $\mathbf{b}_{3}^{*}$. We use the controller (18) with
$\mathbf{R}^{*}=[\mathbf{b}_{1}^{*}\quad\mathbf{b}_{2}^{*}\quad\mathbf{b}_{3}^{*}]$
where:

$\mathbf{b}_{3}^{*}=\frac{\mathbf{R}\mathbf{b}_{\mathbf{v}}}{\|\mathbf{R}\mathbf{b}_{\mathbf{v}}\|},\mathbf{b}_{1}^{*}=\frac{\mathbf{b}_{2}^{\psi}\times\mathbf{b}^{*}_{3}}{\|\mathbf{b}_{2}^{\psi}\times\mathbf{b}^{*}_{3}\|},\mathbf{b}_{2}^{*}=\mathbf{b}_{3}^{*}\times\mathbf{b}_{1}^{*},$
(51)

and $\hat{{\boldsymbol{\omega}}}^{*}=\mathbf{R}^{*\top}\dot{\mathbf{R}}^{*}$
for our tracking task.

Figure 9: Safe navigation of quadrotor system (learned model) in a warehouse:
the dynamic safety margin $\Delta E$, the Lyapunov function ${\cal V}$ (top)
and the distance to the obstacles $\bar{d}(\mathbf{p}(t),{\cal O})$ (bottom),
indicating that the safety constraints are never violated. Figure 10: Tracking
error of a quadrotor system (top to bottom): position, velocity, angle and
angular velocity errors.

We successfully finish the task with the quadrotor while remaining safe for
the entire experiment, as shown in Fig. 9, with similar behavior of the
closed-loop quadrotor system in terms of the safety margin, Lyapunov function
and distance to obstacle compared to Sec. 7.7.3. However, the orientation
tracking error of quadrotor (Fig. 10) is larger than that of hexarotor, as
expected since the quadrotor is underactuated.

###  7.5 Evaluation of our approach against unmodeled noise

In this section, we verify the robustness of our controller against unmodeled
noise on a simulated hexarotor by injecting high frequency noise (e.g.,
propeller vibration) into control inputs and simulating state estimation
errors. In particular, a $4.8$ kHz 6D sinusoidal signal with amplitude $5$ is
generated for high frequency noise. Meanwhile, state estimation errors in
positions, Euler angles, linear and angular velocity are randomly generated
with zero mean and standard deviation, chosen from [69] (position: $0.01$ m,
Euler angle: $0.01$ degree, linear velocity: $0.02$ m/s and angular velocity:
$0.14$ degree/s). We consider the task of stabilizing to a static governor,
i.e. the governor is not moving, with the learned dynamics model: without any
unmodeled noise (base), with high-frequency noise, and with state estimation
error. Fig. 11 plots the Lyapunov function ${\cal V}$ and the safety margin
$\Delta E$ over time. Our controller is not affected significantly from the
high-frequency noise, potentially because the noise’s effect is canceled out
due to its zero mean. Our controller is safe against the state estimation
errors from [69], i.e. $\Delta E>0$ over time, but fails to remains safe, i.e.
$\Delta E<0$ at some times, if we triple the noise deviation.

Figure 11: The Lyapunov function ${\cal V}$ and safety margin $\Delta E$ in
the presence of: high-frequency noise (top), state estimation error with
standard deviation from [69] (middle) and 3x larger (bottom), respectively).

##  8 Conclusion

This paper developed a tracking controller for Hamiltonian systems with
learned dynamics. We employed a neural ODE network to learn translation-
invariant Hamiltonian dynamics on the $SE(3)$ manifold from trajectory data.
The Hamiltonian of the learned system was used to synthesize an energy-shaping
controller and quantify its robustness to modeling errors. A reference
governor was employed to guide the system along a desired reference path using
the trade-off between system energy, disturbance bounds, and distance to
obstacles to guarantee safe tracking. Our results demonstrate that encoding
$SE(3)$ kinematics and Hamiltonian dynamics in the model learning process
achieves more accurate prediction than Gaussian Process regression. Utilizing
the system energy in the control design offers a general approach for
guaranteeing robustness and safety for physical systems and generalizes well
to desired trajectories which are significantly different from the training
data. Future work will focus on disturbance compensation and real experiments.

## ACKNOWLEDGMENT

We gratefully acknowledge support from NSF RI IIS-2007141 and NSF CCF-2112665
(TILOS).

##  9 Proof of Thm. 1

We do not write function arguments explicitly to simplify the notation. We
also introduce the following notation for the components of $\mathbf{e}$ and
${\mathfrak{p}}_{e}$ in (23):

$\displaystyle\mathbf{e}$
$\displaystyle=\begin{bmatrix}\mathbf{e}_{\mathbf{p}}\\\
\mathbf{e}_{\mathbf{R}}\end{bmatrix}=\begin{bmatrix}k_{\mathbf{p}}\mathbf{R}^{\top}\mathbf{p}_{e}\\\
\frac{1}{2}k_{\mathbf{R}}{\rm
Phys.~{}Rev.~{}Lett.}{\mathbf{R}_{e}-\mathbf{R}_{e}^{\top}}^{\vee}\end{bmatrix},$
(52) $\displaystyle{\mathfrak{p}}_{e}$
$\displaystyle=\mathbf{M}\begin{bmatrix}\mathbf{e}_{\mathbf{v}}\\\
\mathbf{e}_{{\boldsymbol{\omega}}}\end{bmatrix}=\mathbf{M}\begin{bmatrix}\mathbf{v}-\mathbf{R}_{e}^{\top}\mathbf{v}^{*}\\\
{\boldsymbol{\omega}}-\mathbf{R}_{e}^{\top}{\boldsymbol{\omega}}^{*}\end{bmatrix}.$

Consider the Lyapunov function candidate in (24):

${\cal
V}=\frac{1}{2}{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}{\mathfrak{p}}_{e}+{\cal
U}_{d}+\rho\frac{d}{dt}{\cal U}_{d},$ (53)

where ${\cal
U}_{d}=\frac{k_{\mathbf{p}}}{2}\|\mathbf{p}_{e}\|^{2}+\frac{k_{\mathbf{R}}}{2}\operatorname{tr}(\mathbf{I}-\mathbf{R}_{e})$.
In the domain ${\cal A}$, we have [70, Prop. 1]:

$k_{\mathbf{R}}^{-2}\|\mathbf{e}_{\mathbf{R}}\|_{2}^{2}\leq\operatorname{tr}(\mathbf{I}-\mathbf{R}_{e})\leq\frac{4k_{\mathbf{R}}^{-2}}{4-\alpha}\|\mathbf{e}_{\mathbf{R}}\|_{2}^{2}.$
(54)

By the chain rule and (23), we have:

$\frac{d}{dt}{\cal U}_{d}=\nabla_{{\mathfrak{q}}_{e}}\\!{\cal
H}_{d}^{\top}\dot{{\mathfrak{q}}}_{e}=\nabla_{{\mathfrak{q}}_{e}}\\!{\cal
H}_{d}^{\top}\mathbf{J}\mathbf{M}^{-1}{\mathfrak{p}}_{e}=\mathbf{e}^{\top}\\!\mathbf{M}^{-1}{\mathfrak{p}}_{e}$
(55)

Using (54) and (55), together with the Cauchy-Schwartz inequality and the sub-
multiplicative property of the Euclidean norm, the Lyapunov function candidate
is bounded as:

$\displaystyle{\cal V}$
$\displaystyle\leq\frac{\lambda_{2}}{2}\lVert{\mathfrak{p}}_{e}\rVert^{2}+\frac{k_{\mathbf{p}}^{-1}}{2}\lVert\mathbf{e}_{\mathbf{p}}\rVert^{2}+\frac{2k_{\mathbf{R}}^{-1}}{4-\alpha}\lVert\mathbf{e}_{\mathbf{R}}\rVert^{2}+\rho\lambda_{2}\lVert\mathbf{e}\rVert\lVert{\mathfrak{p}}_{e}\rVert.$
$\displaystyle{\cal V}$
$\displaystyle\geq\frac{\lambda_{1}}{2}\lVert{\mathfrak{p}}_{e}\rVert^{2}+\frac{k_{\mathbf{p}}^{-1}}{2}\lVert\mathbf{e}_{\mathbf{p}}\rVert^{2}+\frac{k_{\mathbf{R}}^{-1}}{2}\lVert\mathbf{e}_{\mathbf{R}}\rVert^{2}-\rho\lambda_{2}\lVert\mathbf{e}\rVert\lVert{\mathfrak{p}}_{e}\rVert.$

The bounds can be stated compactly in quadratic form using
$\mathbf{z}=[\|\mathbf{e}\|\;\|{\mathfrak{p}}_{e}\|]^{\top}$ and
$\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$ in (26):

$\frac{1}{2}\mathbf{z}^{\top}\mathbf{Q}_{1}\mathbf{z}\leq{\cal
V}\leq\frac{1}{2}\mathbf{z}^{\top}\mathbf{Q}_{2}\mathbf{z}.$ (56)

The time derivative of the Lyapunov candidate satisfies:

$\frac{d}{dt}{\cal
V}={\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}\dot{{\mathfrak{p}}}_{e}+\mathbf{e}^{\top}\mathbf{M}^{-1}{\mathfrak{p}}_{e}+\rho\mathbf{e}^{\top}\mathbf{M}^{-1}\dot{{\mathfrak{p}}}_{e}+\rho\dot{\mathbf{e}}^{\top}\mathbf{M}^{-1}{\mathfrak{p}}_{e}.$

The term $\dot{{\mathfrak{p}}}_{e}$ is from (23). The term $\dot{\mathbf{e}}$
is obtained from (52):

$\displaystyle\dot{\mathbf{e}}$
$\displaystyle=\begin{bmatrix}\dot{\mathbf{e}}_{\mathbf{p}}\\\
\dot{\mathbf{e}}_{\mathbf{R}}\end{bmatrix}=\begin{bmatrix}-\hat{{\boldsymbol{\omega}}}\mathbf{e}_{\mathbf{p}}+k_{\mathbf{p}}\mathbf{e}_{\mathbf{v}}\\\
k_{\mathbf{R}}\mathbf{E}_{\mathbf{R}}\mathbf{e}_{{\boldsymbol{\omega}}}\end{bmatrix}$
(57) $\displaystyle=-\begin{bmatrix}\hat{{\boldsymbol{\omega}}}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}\end{bmatrix}\mathbf{e}+\begin{bmatrix}k_{\mathbf{p}}\mathbf{I}&\mathbf{0}\\\
\mathbf{0}&k_{\mathbf{R}}\mathbf{E}_{\mathbf{R}}\end{bmatrix}\mathbf{M}^{-1}{\mathfrak{p}}_{e},$

where
$\mathbf{E}_{\mathbf{R}}=\frac{1}{2}\left[\operatorname{tr}(\mathbf{R}_{e}^{\top})\mathbf{I}-\mathbf{R}_{e}^{\top}\right]$
satisfies $\|\mathbf{E}_{\mathbf{R}}\|_{2}\leq 1$ [64, Prop. 1]. Hence, we
have:

$\displaystyle\frac{d}{dt}{\cal
V}=-{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}\mathbf{K}_{\mathbf{d}}\mathbf{M}^{-1}{\mathfrak{p}}_{e}+{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}\mathbf{d}$
$\displaystyle\quad-\rho\mathbf{e}^{\top}\mathbf{M}^{-1}\mathbf{e}-\rho\mathbf{e}^{\top}\mathbf{M}^{-1}\mathbf{K}_{\mathbf{d}}\mathbf{M}^{-1}{\mathfrak{p}}_{e}+\rho\mathbf{e}^{\top}\mathbf{M}^{-1}\mathbf{d}$
$\displaystyle\quad-\rho{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}\begin{bmatrix}\hat{{\boldsymbol{\omega}}}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}\end{bmatrix}\mathbf{e}+\rho{\mathfrak{p}}_{e}^{\top}\mathbf{M}^{-1}\begin{bmatrix}k_{\mathbf{p}}\mathbf{I}&\mathbf{0}\\\
\mathbf{0}&k_{\mathbf{R}}\mathbf{E}_{\mathbf{R}}\end{bmatrix}\mathbf{M}^{-1}{\mathfrak{p}}_{e}.$

To find an upper bound on $\frac{d}{dt}{\cal V}$, we need a few intermediate
steps. First, on the domain ${\cal A}$, we have:

$\left\lVert\begin{bmatrix}\hat{{\boldsymbol{\omega}}}&\mathbf{0}\\\
\mathbf{0}&\mathbf{0}\end{bmatrix}\right\rVert_{2}=\lVert\hat{{\boldsymbol{\omega}}}\rVert_{2}=\lVert{\boldsymbol{\omega}}\rVert\leq\|\mathbf{M}^{-1}{\mathfrak{p}}\|\leq\lambda_{2}\beta.$
(58)

Second, an upper bound on

$\xi_{1}\coloneqq-\lambda_{\min}(\mathbf{K}_{\mathbf{d}})\lVert\mathbf{M}^{-1}{\mathfrak{p}}_{e}\rVert^{2}+\lVert\mathbf{M}^{-1}{\mathfrak{p}}_{e}\rVert\lVert\mathbf{d}\rVert$
(59)

can be found using Young’s inequality [43]:

$-\epsilon\lVert\mathbf{a}\rVert^{2}+\eta\lVert\mathbf{a}\rVert\lVert\mathbf{b}\rVert\leq-\frac{\epsilon}{2}\lVert\mathbf{a}\rVert^{2}+\frac{\eta^{2}}{2\epsilon}\lVert\mathbf{b}\rVert^{2}$
(60)

with $\epsilon=\lambda_{\min}(\mathbf{K}_{d})$, $\eta=1$,
$\mathbf{a}=\mathbf{M}^{-1}{\mathfrak{p}}_{e}$, $\mathbf{b}=\mathbf{d}$:

$\displaystyle\xi_{1}$
$\displaystyle\leq-\frac{\lambda_{\min}(\mathbf{K}_{d})}{2}\lVert\mathbf{M}^{-1}{\mathfrak{p}}_{e}\rVert^{2}+\frac{1}{2\lambda_{\min}(\mathbf{K}_{d})}\lVert\mathbf{d}\rVert^{2}.$
(61)

Similarly, we have:

$\xi_{2}\coloneqq-\lambda_{1}\lVert\mathbf{e}\rVert^{2}+\lambda_{2}\lVert\mathbf{e}\rVert\lVert\mathbf{d}\rVert\leq-\frac{\lambda_{1}}{2}\lVert\mathbf{e}\rVert^{2}+\frac{\lambda_{2}^{2}}{2\lambda_{1}}\lVert\mathbf{d}\rVert^{2}.$
(62)

Using (58), (61), and (62), $\frac{d}{dt}{\cal V}$ is bounded by:

$\frac{d}{dt}{\cal
V}\leq-\frac{1}{2}\mathbf{z}^{\top}\mathbf{Q}_{3}\mathbf{z}+k_{\gamma}\lVert\mathbf{d}\rVert^{2},$
(63)

where the elements of $\mathbf{Q}_{3}$ are provided in (27) and
$k_{\gamma}=\frac{1}{2\lambda_{\min}(\mathbf{K}_{\mathbf{d}})}+\frac{\rho\lambda_{2}^{2}}{2\lambda_{1}}$.
Since the parameters $\rho$, $k_{\mathbf{p}}$, $k_{\mathbf{R}}$,
$\mathbf{K}_{\mathbf{d}}$ can be chosen arbitrarily, there exists some choice
that ensures the matrices $\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$, $\mathbf{Q}_{3}$
are positive definite as shown below. The inequalities in (25) are obtained
from (56) and (63) using the Rayleigh-Ritz inequality.

#### Region of Attraction

We use the invariant sets ${\cal S}_{c}=\left\\{\mathbf{x}\mid{\cal
V}(\mathbf{x},\mathbf{x}^{*})\leq c\right\\}$ induced by the Lyapunov function
to restrict the error dynamics inside the domain ${\cal A}$ and estimate the
region of attraction.

We determine $c_{1}\geq 0$ such that $\dot{{\cal V}}$ is positive on ${\cal
S}_{c_{1}}$. From (25), $\dot{{\cal V}}$ is positive when
$k_{\gamma}\delta_{\mathbf{d}}^{2}-k_{3}\lVert\mathbf{z}\rVert^{2}\geq 0$,
which happens when $\frac{{\cal
V}}{k_{2}}\leq\frac{k_{\gamma}}{k_{3}}\delta_{\mathbf{d}}^{2}$. Hence, with
$c_{1}=k_{2}k_{\gamma}\delta_{\mathbf{d}}^{2}/k_{3}$, we have $\dot{{\cal
V}}\geq 0$ on ${\cal S}_{c_{1}}$. Then, we determine $c_{2}\geq 0$ such that
${\cal S}_{c_{2}}\subseteq{\cal A}$. From (54) and (56), we have:

$\frac{4-\alpha}{4}k_{\mathbf{R}}^{2}\operatorname{tr}(\mathbf{I}-\mathbf{R}_{e})\leq\lVert\mathbf{e}_{\mathbf{R}}\rVert^{2}\leq\lVert\mathbf{z}\rVert^{2}\leq\frac{{\cal
V}}{k_{1}}.$ (64)

Hence, if ${\cal V}\leq\frac{1}{4}k_{1}k_{\mathbf{R}}^{2}\alpha{\rm
Phys.~{}Rev.~{}Lett.}{4-\alpha}$, then
$\operatorname{tr}(\mathbf{I}-\mathbf{R}_{e})\leq\alpha$. Similarly, if ${\cal
V}\leq k_{1}\beta^{2}$, then
$\lVert{\mathfrak{p}}_{e}\rVert^{2}\leq\lVert\mathbf{z}\rVert^{2}\leq\frac{{\cal
V}}{k_{1}}\leq\beta^{2}$. Hence, to ensure that ${\cal
S}_{c_{2}}\subseteq{\cal A}$, we define $c_{2}$ as:

$c_{2}\coloneqq
k_{1}\min\left\\{k_{\mathbf{R}}^{2}\alpha(4-\alpha)/4,\beta^{2}\right\\}.$
(65)

To ensure that $c_{1}<c_{2}$, the disturbance bound $\delta_{\mathbf{d}}$ must
satisfy $\delta_{\mathbf{d}}<\sqrt{\frac{c_{2}k_{3}}{k_{2}k_{\gamma}}}$. Then,
any closed-loop system trajectory that starts in ${\cal S}_{c_{2}}$ converges
exponentially to ${\cal S}_{c_{1}}$ and remains within it. Recall that
$\mathbf{e}_{\mathbf{p}}=k_{\mathbf{p}}\mathbf{R}^{\top}\mathbf{p}_{e}$ and
from (56):

$k_{\mathbf{p}}^{2}\lVert\mathbf{p}_{e}\rVert^{2}=\lVert\mathbf{e}_{\mathbf{p}}\rVert^{2}\leq\lVert\mathbf{e}\rVert^{2}\leq\lVert\mathbf{z}\rVert^{2}\leq\frac{{\cal
V}}{k_{1}}.$ (66)

Hence, on ${\cal S}_{c_{1}}$, $\lVert\mathbf{p}_{e}\rVert^{2}\leq
c_{1}/(k_{1}k_{\mathbf{p}}^{2})$ and the uniform ultimate bound on the
position error trajectory in (29) is satisfied.

#### Design Parameter Choice

We propose a systematic way to select parameters $\rho$, $k_{\mathbf{p}}$,
$k_{\mathbf{R}}$, $\mathbf{K}_{\mathbf{d}}$, ensuring that the matrices
$\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$, $\mathbf{Q}_{3}$ are positive definite.
Suppose $k_{\mathbf{p}}<\frac{4-\alpha}{4}k_{\mathbf{R}}$ and
$\mathbf{K}_{\mathbf{d}}=\gamma_{d}\mathbf{I}$ for some $\gamma_{d}>0$, then
we have

$\displaystyle\mathbf{Q}_{1}$
$\displaystyle=\begin{bmatrix}k_{\mathbf{R}}^{-1}&-\rho\lambda_{2}\\\
-\rho\lambda_{2}&\lambda_{1}\end{bmatrix}$ $\displaystyle\mathbf{Q}_{2}$
$\displaystyle=\begin{bmatrix}k_{\mathbf{p}}^{-1}&\rho\lambda_{2}\\\
\rho\lambda_{2}&\lambda_{2}\end{bmatrix}$ (67) $\displaystyle q_{2}$
$\displaystyle=-\rho\lambda_{2}^{2}{\rm
Phys.~{}Rev.~{}Lett.}{\gamma_{d}+\beta}$ $\displaystyle q_{3}$
$\displaystyle=\gamma_{d}\lambda_{1}^{2}-2\rho\lambda_{2}^{2}k_{\mathbf{R}}.$

To guarantee the positive definiteness of $\mathbf{Q}_{1}$, $\mathbf{Q}_{2}$,
$\mathbf{Q}_{3}$, the following requirements must be satisfied:

$\displaystyle\frac{\lambda_{1}}{k_{\mathbf{R}}}-\rho^{2}\lambda_{2}^{2}>0,\;\frac{\lambda_{2}}{k_{\mathbf{p}}}-\rho^{2}\lambda_{2}^{2}>0,\;\gamma_{d}\lambda_{1}^{2}-2\rho\lambda_{2}^{2}k_{\mathbf{R}}>0$
$\displaystyle\rho\lambda_{1}{\rm
Phys.~{}Rev.~{}Lett.}{\gamma_{d}\lambda_{1}^{2}-2\rho\lambda_{2}^{2}k_{\mathbf{R}}}-\rho^{2}\lambda_{2}^{4}{\rm
Phys.~{}Rev.~{}Lett.}{\gamma_{d}+\beta}^{2}>0.$

All these constraints put upper bounds on $\rho$:

$\rho\leq\min\left\\{\sqrt{\frac{\lambda_{1}}{k_{\mathbf{R}}\lambda_{2}^{2}}},\;\sqrt{\frac{1}{k_{\mathbf{p}}\lambda_{2}}},\;\frac{\gamma_{d}\lambda_{1}^{2}}{2k_{\mathbf{R}}\lambda_{2}^{2}},\;\bar{\rho}_{\mathbf{Q}_{3}}\right\\},$
(68)

where
$\bar{\rho}_{\mathbf{Q}_{3}}=\frac{\gamma_{d}\lambda_{1}^{3}}{\lambda_{2}^{2}\left[2\lambda_{1}k_{\mathbf{R}}+\lambda_{2}^{2}(\gamma_{d}+\beta)^{2}\right]}$.

## References

  * [1] A. D. Ames, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs with application to adaptive cruise control,” in _IEEE Conference on Decision and Control (CDC)_ , 2014, pp. 6271–6278.
  * [2] S. Shalev-Shwartz, S. Shammah, and A. Shashua, “Safe, multi-agent, reinforcement learning for autonomous driving,” _arXiv preprint arXiv:1610.03295_ , 2016.
  * [3] A. D. Ames, K. Galloway, K. Sreenath, and J. W. Grizzle, “Rapidly exponentially stabilizing control Lyapunov functions and hybrid zero dynamics,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 59, no. 4, pp. 876–891, 2014.
  * [4] M. C. Yip and D. B. Camarillo, “Model-less feedback control of continuum manipulators in constrained environments,” _IEEE Transactions on Robotics (T-RO)_ , vol. 30, no. 4, pp. 880–889, 2014.
  * [5] M. P. Deisenroth, D. Fox, and C. E. Rasmussen, “Gaussian Processes for Data-Efficient Learning in Robotics and Control,” _IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI)_ , vol. 37, no. 2, pp. 408–423, 2015.
  * [6] J. Kabzan, L. Hewing, A. Liniger, and M. N. Zeilinger, “Learning-based model predictive control for autonomous racing,” _IEEE Robotics and Automation Letters (RA-L)_ , vol. 4, no. 4, pp. 3363–3370, 2019.
  * [7] L. Hewing, J. Kabzan, and M. N. Zeilinger, “Cautious model predictive control using gaussian process regression,” _IEEE Transactions on Control Systems Technology_ , vol. 28, no. 6, 2019.
  * [8] M. Raissi, P. Perdikaris, and G. E. Karniadakis, “Multistep neural networks for data-driven discovery of nonlinear dynamical systems,” _arXiv preprint arXiv:1801.01236_ , 2018.
  * [9] K. Chua, R. Calandra, R. McAllister, and S. Levine, “Deep reinforcement learning in a handful of trials using probabilistic dynamics models,” in _Advances in Neural Information Processing Systems (NeurIPS)_ , 2018.
  * [10] M. Lutter, K. Listmann, and J. Peters, “Deep Lagrangian Networks for end-to-end learning of energy-based control for under-actuated systems,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2019.
  * [11] Y. D. Zhong, B. Dey, and A. Chakraborty, “Symplectic ODE-Net: learning Hamiltonian dynamics with control,” in _International Conference on Learning Representations (ICLR)_ , 2019.
  * [12] T. Duong and N. Atanasov, “Hamiltonian-based Neural ODE Networks on the SE(3) Manifold For Dynamics Learning and Control,” in _Proceedings of Robotics: Science and Systems_ , Virtual, July 2021.
  * [13] A. I. Lurie, _Analytical mechanics_. Springer Science & Business Media, 2013.
  * [14] D. D. Holm, _Geometric Mechanics_. World Scientific Publishing Company, 2008.
  * [15] R. T. Chen, Y. Rubanova, J. Bettencourt, and D. Duvenaud, “Neural ordinary differential equations,” in _Advances in Neural Information Processing Systems (NeurIPS)_ , 2018.
  * [16] T. Duong and N. Atanasov, “Learning Adaptive Control for SE(3) Hamiltonian Dynamics,” _arXiv preprint:2109.09974_ , 2021.
  * [17] A. Van Der Schaft and D. Jeltsema, “Port-Hamiltonian systems theory: An introductory overview,” _Foundations and Trends in Systems and Control_ , vol. 1, no. 2-3, 2014.
  * [18] S. Saemundsson, A. Terenin, K. Hofmann, and M. Deisenroth, “Variational integrator networks for physically structured embeddings,” in _International Conference on Artificial Intelligence and Statistics_. PMLR, 2020, pp. 3078–3087.
  * [19] A. Havens and G. Chowdhary, “Forced variational integrator networks for prediction and control of mechanical systems,” in _Learning for Dynamics and Control_. PMLR, 2021, pp. 1142–1153.
  * [20] F. Borrelli, A. Bemporad, and M. Morari, _Predictive control for linear and hybrid systems_. Cambridge University Press, 2017.
  * [21] L. Grüne and J. Pannek, _Nonlinear model predictive control_. Springer, 2017.
  * [22] J. M. Bravo, T. Alamo, and E. F. Camacho, “Robust mpc of constrained discrete-time nonlinear systems based on approximated reachable sets,” _Automatica_ , vol. 42, no. 10, pp. 1745–1751, 2006.
  * [23] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, “Constrained model predictive control: Stability and optimality,” _Automatica_ , vol. 36, no. 6, pp. 789–814, 2000.
  * [24] S. L. Herbert, M. Chen, S. Han, S. Bansal, J. F. Fisac, and C. J. Tomlin, “FaSTrack: A modular framework for fast and guaranteed safe motion planning,” in _IEEE Conference on Decision and Control (CDC)_ , 2017, pp. 1517–1522.
  * [25] A. Majumdar and R. Tedrake, “Funnel libraries for real-time robust feedback motion planning,” _The International Journal of Robotics Research (IJRR)_ , vol. 36, no. 8, pp. 947–982, 2017.
  * [26] S. Kousik, S. Vaskov, F. Bu, M. Johnson-Roberson, and R. Vasudevan, “Bridging the gap between safety and real-time performance in receding-horizon trajectory design for mobile robots,” _The International Journal of Robotics Research (IJRR)_ , 2020.
  * [27] J. F. Fisac, M. Chen, C. J. Tomlin, and S. S. Sastry, “Reach-avoid problems with time-varying dynamics, targets and constraints,” in _International conference on hybrid systems: computation and control (HSCC)_ , 2015, pp. 11–20.
  * [28] M. Chen, S. Herbert, and C. J. Tomlin, “Exact and efficient Hamilton-Jacobi guaranteed safety analysis via system decomposition,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2017, pp. 87–92.
  * [29] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 62, no. 8, pp. 3861–3876, 2017\.
  * [30] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, “Control barrier functions: Theory and applications,” in _IEEE European Control Conference (ECC)_ , 2019, pp. 3420–3431.
  * [31] X. Xu, “Constrained control of input–output linearizable systems using control sharing barrier functions,” _Automatica_ , vol. 87, pp. 195–201, 2018.
  * [32] K. Garg and D. Panagou, “Characterization of domain of fixed-time stability under control input constraints,” in _IEEE American Control Conference (ACC)_ , 2021, pp. 2272–2277.
  * [33] A. Bemporad, “Reference governor for constrained nonlinear systems,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 43, no. 3, pp. 415–419, 1998.
  * [34] I. Kolmanovsky, E. Garone, and S. Di Cairano, “Reference and command governors: A tutorial on their theory and automotive applications,” in _IEEE American Control Conference (ACC)_ , 2014, pp. 226–241.
  * [35] E. Garone and M. M. Nicotra, “Explicit reference governor for constrained nonlinear systems,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 61, no. 5, pp. 1379–1384, 2016.
  * [36] O. Arslan and D. E. Koditschek, “Smooth extensions of feedback motion planners via reference governors,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2017.
  * [37] Z. Li, O. Arslan, and N. Atanasov, “Fast and Safe Path-Following Control using a State-Dependent Directional Metric,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [38] L. Brunke, M. Greeff, A. W. Hall, Z. Yuan, S. Zhou, J. Panerati, and A. P. Schoellig, “Safe learning in robotics: From learning-based control to safe reinforcement learning,” _Annual Review of Control, Robotics, and Autonomous Systems_ , 2021.
  * [39] P. A. Ioannou and J. Sun, _Robust adaptive control_. Prentice Hall, 1996.
  * [40] A. Gahlawat, P. Zhao, A. Patterson, N. Hovakimyan, and E. Theodorou, “L1-gp: L1 adaptive control with bayesian learning,” in _Learning for Dynamics and Control_. PMLR, 2020, pp. 826–837.
  * [41] T. Lee, M. Leok, and N. H. McClamroch, “Nonlinear robust tracking control of a quadrotor UAV on SE(3),” _Asian Journal of Control_ , vol. 15, no. 2, pp. 391–408, 2013.
  * [42] S. P. Nageshrao, G. A. Lopes, D. Jeltsema, and R. Babuška, “Port-hamiltonian systems in adaptive and learning control: A survey,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 61, no. 5, pp. 1223–1238, 2015.
  * [43] M. Ryalat and D. S. Laila, “A robust ida-pbc approach for handling uncertainties in underactuated mechanical systems,” _IEEE Transactions on Automatic Control (TAC)_ , vol. 63, no. 10, pp. 3495–3502, 2018.
  * [44] A. K. Akametalu, S. Ghosh, J. F. Fisac, and C. J. Tomlin, “A minimum discounted reward hamilton-jacobi formulation for computing reachable sets,” _arXiv preprint arXiv:1809.00706_ , 2018.
  * [45] Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles,” _Vehicle System Dynamics_ , vol. 52, no. 6, pp. 802–823, 2014.
  * [46] C. J. Ostafew, A. P. Schoellig, and T. D. Barfoot, “Robust constrained learning-based nmpc enabling reliable mobile robot path tracking,” _The International Journal of Robotics Research (IJRR)_ , vol. 35, no. 13, pp. 1547–1563, 2016.
  * [47] D. D. Fan, A.-a. Agha-mohammadi, and E. A. Theodorou, “Deep learning tubes for tube mpc,” _arXiv preprint arXiv:2002.01587_ , 2020.
  * [48] G. Joshi and G. Chowdhary, “Deep model reference adaptive control,” in _IEEE Conference on Decision and Control (CDC)_ , 2019, pp. 4601–4608.
  * [49] M. Ryalat, D. S. Laila, and M. M. Torbati, “Integral ida-pbc and pid-like control for port-controlled hamiltonian systems,” in _IEEE American Control Conference (ACC)_ , 2015, pp. 5365–5370.
  * [50] J. Á. Acosta, M. Sanchez, and A. Ollero, “Robust control of underactuated aerial manipulators via IDA-PBC,” in _IEEE Conference on Decision and Control (CDC)_ , 2014, pp. 673–678.
  * [51] Z. Li, T. Duong, and N. Atanasov, “Safe Autonomous Navigation for Systems with Learned SE(3) Hamiltonian Dynamics,” in _Learning for Dynamics and Control_. PMLR, 2022.
  * [52] T. Lee, M. Leok, and N. H. McClamroch, _Global formulations of Lagrangian and Hamiltonian dynamics on manifolds_. Springer, 2017.
  * [53] Y. D. Zhong, B. Dey, and A. Chakraborty, “Dissipative SymODEN: Encoding Hamiltonian dynamics with dissipation and control into deep learning,” in _ICLR 2020 Workshop on Integration of Deep Neural Models and Differential Equations_ , 2020.
  * [54] R. Dandekar, K. Chung, V. Dixit, M. Tarek, A. Garcia-Valadez, K. V. Vemula, and C. Rackauckas, “Bayesian neural ordinary differential equations,” _arXiv preprint arXiv:2012.07244_ , 2020.
  * [55] W. Xu, R. T. Q. Chen, X. Li, and D. Duvenaud, “Infinitely deep bayesian neural networks with stochastic differential equations,” in _Proceedings of The 25th International Conference on Artificial Intelligence and Statistics_ , ser. Proceedings of Machine Learning Research, 28–30 Mar 2022, pp. 721–738.
  * [56] X. Liu, T. Xiao, S. Si, Q. Cao, S. Kumar, and C.-J. Hsieh, “Neural sde: Stabilizing neural ode networks with stochastic noise,” _arXiv preprint arXiv:1906.02355_ , 2019.
  * [57] M. Heinonen, C. Yildiz, H. Mannerström, J. Intosalmi, and H. Lähdesmäki, “Learning unknown ODE models with Gaussian processes,” in _Proceedings of the 35th International Conference on Machine Learning_ , ser. Proceedings of Machine Learning Research, J. Dy and A. Krause, Eds., vol. 80, 10–15 Jul 2018, pp. 1959–1968.
  * [58] R. Ortega, A. Van Der Schaft, B. Maschke, and G. Escobar, “Interconnection and damping assignment passivity-based control of port-controlled Hamiltonian systems,” _Automatica_ , vol. 38, no. 4, pp. 585–596, 2002.
  * [59] G. Blankenstein, R. Ortega, and A. J. Van Der Schaft, “The matching conditions of controlled Lagrangians and IDA-passivity based control,” _International Journal of Control_ , vol. 75, no. 9, pp. 645–665, 2002.
  * [60] F. Gomez-Estern, R. Ortega, F. R. Rubio, and J. Aracil, “Stabilization of a class of underactuated mechanical systems via total energy shaping,” in _IEEE Conference on Decision and Control_ , vol. 2, 2001, pp. 1137–1143.
  * [61] J. A. Acosta, R. Ortega, A. Astolfi, and A. D. Mahindrakar, “Interconnection and damping assignment passivity-based control of mechanical systems with underactuation degree one,” _IEEE Transactions on Automatic Control_ , vol. 50, no. 12, pp. 1936–1955, 2005.
  * [62] B. Yüksel, C. Secchi, H. H. Bülthoff, and A. Franchi, “Reshaping the physical properties of a quadrotor through ida-pbc and its application to aerial physical interaction,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2014.
  * [63] E. D. Sontag, “Input to state stability: Basic concepts and results,” in _Nonlinear and optimal control theory_. Springer, 2008, pp. 163–220.
  * [64] T. Lee, M. Leok, and N. H. McClamroch, “Geometric tracking control of a quadrotor UAV on SE(3),” in _IEEE Conference on Decision and Control (CDC)_ , 2010.
  * [65] A. F. Filippov, _Differential equations with discontinuous righthand sides: control systems_. Springer Science & Business Media, 1988, vol. 18.
  * [66] Z. Li, T. Duong, and N. Atanasov, “Safe Robot Navigation in Cluttered Environments using Invariant Ellipsoids and a Reference Governor,” _arXiv preprint arXiv:2005.06694_ , 2020.
  * [67] N. Kingry, L. Towers, Y.-C. Liu, Y. Zu, Y. Wang, B. Staheli, Y. Katagiri, S. Cook, and R. Dai, “Design, modeling and control of a solar-powered quadcopter,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2018, pp. 1251–1258.
  * [68] S. Rajappa, M. Ryll, H. H. Bülthoff, and A. Franchi, “Modeling, control and design optimization for a fully-actuated hexarotor aerial vehicle with tilted propellers,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2015.
  * [69] M. L. Hoang, M. Carratù, V. Paciello, and A. Pietrosanto, “Noise Attenuation on IMU Measurement For Drone Balance by Sensor Fusion,” in _IEEE International Instrumentation and Measurement Technology Conference (I2MTC)_ , 2021, pp. 1–6.
  * [70] T. Lee, “Robust Adaptive Attitude Tracking on SO(3) with an Application to a Quadrotor UAV,” _IEEE Transactions on Control Systems Technology_ , vol. 21, no. 5, pp. 1924–1930, 2013.

[]Zhichao Li is a PhD student in Electrical and Computer Engineering at the
University of California San Diego, La Jolla, CA, USA. He received a B.Eng.
degree from the School of Astronautics, Northwestern Polytechnical University,
Xi’an, Shaanxi, China in 2013 and an M.S. degree in Electrical Engineering
from the School of Electrical, Computer and Energy Engineering, Arizona State
University, Tempe, AZ, in 2016. His research interests include control theory
and motion planning with applications to mobile robots.

[]Thai Duong is a PhD student in Electrical and Computer Engineering at the
University of California San Diego, CA, USA. He received a B.S. degree in
Electronics and Telecommunications from Hanoi University of Science and
Technology, Hanoi, Vietnam in 2011 and an M.S. degree in Electrical and
Computer Engineering from Oregon State University, Corvallis, OR, in 2013. His
research interests include machine learning with applications to robotics,
mapping and active exploration using mobile robots, robot dynamics learning,
and decision making under uncertainty.

[]Nikolay Atanasov is an Assistant Professor of Electrical and Computer
Engineering at the University of California San Diego, La Jolla, CA, USA. He
obtained a B.S. degree in Electrical Engineering from Trinity College,
Hartford, CT, USA in 2008 and M.S. and Ph.D. degrees in Electrical and Systems
Engineering from the University of Pennsylvania, Philadelphia, PA, USA in 2012
and 2015, respectively. His research focuses on robotics, control theory, and
machine learning, applied to active perception problems for ground and aerial
robots. He works on probabilistic models that unify geometry and semantics in
simultaneous localization and mapping (SLAM), as well as optimal control and
reinforcement learning techniques for minimizing uncertainty in these models.
Dr. Atanasov’s work has been recognized by the Joseph and Rosaline Wolf award
for the best Ph.D. dissertation in Electrical and Systems Engineering at the
University of Pennsylvania in 2015, the best conference paper award at the
2017 IEEE International Conference on Robotics and Automation (ICRA), and a
2021 NSF CAREER award.