# Output Feedback Tube MPC-Guided Data Augmentation for
Robust, Efficient Sensorimotor Policy Learning

Andrea Tagliabue, Jonathan P. How All the authors are with the MIT Department
of Aeronautics and Astronautics. {atagliab<EMAIL_ADDRESS>
###### Abstract

Imitation learning (IL) can generate computationally efficient sensorimotor
policies from demonstrations provided by computationally expensive model-based
sensing and control algorithms. However, commonly employed IL methods are
often data-inefficient, requiring the collection of a large number of
demonstrations and producing policies with limited robustness to
uncertainties. In this work, we combine IL with an output feedback robust tube
model predictive controller (RTMPC) to co-generate demonstrations and a data
augmentation strategy to efficiently learn neural network-based sensorimotor
policies. Thanks to the augmented data, we reduce the computation time and the
number of demonstrations needed by IL, while providing robustness to sensing
and process uncertainty. We tailor our approach to the task of learning a
trajectory tracking visuomotor policy for an aerial robot, leveraging a 3D
mesh of the environment as part of the data augmentation process. We
numerically demonstrate that our method can learn a robust visuomotor policy
from a single demonstration—a two-orders of magnitude improvement in
demonstration efficiency compared to existing IL methods.

##  I INTRODUCTION

Imitation learning [1, 2, 3] is increasingly employed to generate
computationally-efficient policies from computationally-expensive model-based
sensing [4, 5] and control [6, 7, 8] algorithms for onboard deployment. The
key to this method is to leverage the inference speed of deep neural networks,
which are trained to imitate a set of task-relevant expert demonstrations
collected from the model-based algorithms. This approach has been used to
generate efficient sensorimotor policies [9, 10, 7, 11, 12] capable of
producing control commands from raw sensory data, bypassing the computational
cost of control and state estimation, while providing benefits in terms of
latency and robustness. Such sensorimotor policies have demonstrated
impressive performance on a variety of tasks, including agile flight [7] and
driving [13].

However, one of the fundamental limitations of existing IL methods employed to
produce sensorimotor policies (e.g., Behavior Cloning (BC) [1], DAgger [2]) is
the overall number of demonstrations that must be collected from the model-
based algorithm. This inefficiency hinders the possibility of generating
policies by directly collecting data from the real robot, requiring the user
to rely on accurate simulators. Furthermore, the need to repeatedly query a
computationally expensive expert introduces high computational costs during
training, requiring expensive training equipment. The fundamental cause of
these inefficiencies is the need to achieve robustness to sensing noise,
disturbances, and modeling errors encountered during deployment, which can
cause deviations of the policy’s state distribution from its training
distribution—an issue known as covariate shift.

Figure 1: Proposed visuomotor policy learning approach. We collect
demonstrations from an output feedback robust tube MPC, which accounts for the
effects of process and sensing uncertainty via its tube section $\mathbb{Z}$.
We use the tube to obtain a data augmentation strategy, employing a 3D mesh of
the environment created from images $\mathcal{I}_{t}$ captured by an onboard
camera during the demonstration collection phase. The augmented synthetic
images $\mathcal{I}_{t}^{+}$ correspond to states sampled inside the tube, and
the corresponding actions are obtained via the ancillary controller, part of
the tube MPC framework.

Strategies based on matching the disturbances between the training and
deployment domains (e.g., Domain Randomization (DR) [14]) improve the
robustness of the learned policy, but do not address the demonstration and
computational efficiency challenges. Data augmentation approaches based on
generating synthetic sensory measurements and corresponding stabilizing
actions have shown promising results [15, 16]. However, they rely on
handcrafted heuristics for the augmentation procedure and have been mainly
studied on lower-dimensional tasks (e.g., steering a 2D Dubins car).

In this work, we propose a new framework that leverages a model-based robust
controller, an output feedback RTMPC, to provide robust demonstrations and a
corresponding data augmentation strategy for efficient sensorimotor policy
learning. Specifically, we extend our previous work where we show that RTMPC
can be leveraged to generate augmented data that enables efficient learning of
a motor control policy [6]. This new work, summarized in Figure 1, employs an
output feedback variant of RTMPC and a model of the sensory observations to
design a data augmentation strategy that also accounts for the uncertainty
caused by sensing imperfections during the demonstration collection phase.
This approach enables efficient, robust sensorimotor learning from a few
demonstrations and relaxes our previous assumption [6] that state information
is available at training and deployment time.

We tailor our method to the context of learning a visuomotor policy, capable
of robustly tracking given trajectories while using images from an onboard
camera to control the position of an aerial robot. Our approach leverages a 3D
mesh of the environment reconstructed from images to generate the observation
model needed for the proposed augmentation strategy. This technique is
additionally well-suited to be used with photorealistic, data-driven
simulation engines such as FlightMare [17] and FlightGoggles [18], or SLAM and
state estimation pipelines that directly produce 3D dense/mesh reconstructions
of the environment, such as Kimera [19].

Contributions. In summary, our work presents the following contributions:  a)
We introduce a new data augmentation strategy, an extension of our previous
work [6], enabling efficient and robust learning of a sensorimotor policy,
capable to generate control actions directly from raw sensor measurements
(e.g., images) instead of state estimates. Our approach is grounded in the
output feedback RTMPC framework theory, unlike previous methods that rely on
handcrafted heuristics, and leverages a 3D mesh of the environment to generate
augmented data.  b) We demonstrate our methodology in the context of
visuomotor policy learning for an aerial robot, showing that it can track a
trajectory from raw images with high robustness ($>90\%$ success rate) after a
single demonstration, despite sensory noise and disturbances.  c) We open-
source our framework, available at https://github.com/andretag.

##  II RELATED WORKS

Sensorimotor policy learning by imitating model-based experts. Learning a
sensorimotor policy by imitating the output of a model-based expert algorithm
bypasses the computational cost of planning [8], control [6] and state
estimation [10], with the potential of achieving increased robustness [7, 20]
and reduced latency with respect to conventional autonomy pipelines. The
sensory input typically employed is based on raw images [3, 12, 21] or pre-
processed representations, such as feature tracks [7], depth maps [8], or
intermediate layers of a CNN [22]. Vision is often complemented with
proprioceptive information, such as the one provided by IMUs [7] or wheel
encoders [21]. These approaches showcase the advantages of sensorimotor policy
learning but do not leverage any data augmentation strategy. Consequently,
they query the expert many times during the data collection phase, increasing
time, number of demonstrations and computational effort to obtain the policy.

Data augmentation for visuomotor and sensorimotor learning. Traditional data
augmentation strategies for visuomotor policy learning have focused on
increasing a policy’s generalization ability by applying perturbations [23] or
noise [24] directly in image space, without modifying the corresponding
action. These methods do not directly address covariate shift issues caused by
process uncertainties. The self-driving literature has developed a second
class of visuomotor data augmentation strategies [25, 15, 16] capable of
compensating covariate-shift issues. This class relies instead on first
generating different views from synthetic cameras [15, 16] or data-driven
simulators [25] and then computing a corresponding action via a handcrafted
controller. These methods, however, rely on heuristics to establish relevant
augmented views and the corresponding control action. Our work provides a more
general methodology for data augmentation and demonstrates it on a quadrotor
system (higher-dimensional than the planar self-driving car models).

Output feedback RTMPC. Model predictive control [26] leverages a model of the
system dynamics to generate actions that take into account state and actuation
constraints. This is achieved by solving a constrained optimization problem
along a predefined temporal horizon, using the model to predict the effects of
future actions. Robust variants of MPC, such as RTMPC, usually assume that the
system is subject to additive, bounded process uncertainty (e.g.,
disturbances, model errors). As a consequence, they modify nominal plans by
either  a) assuming a worst-case disturbance [27, 28], or  b) employing an
auxiliary (ancillary) controller. This controller maintains the system within
some distance (“cross-section” of a tube) from the nominal plan regardless of
the realization of the disturbances [29, 30].  Output feedback RTMPC [31, 32,
33] also accounts for the effects of sensing uncertainty (e.g., sensing noise,
imperfect state estimation). Our work relies on an output feedback RTMPC [32,
33] to generate demonstrations and a data augmentation strategy. However,
thanks to the proposed imitation learning strategy, our approach does not
require solving the optimization problem online, reducing the onboard
computational cost.

##  III PROBLEM STATEMENT

Our goal is to generate a deep neural network sensorimotor policy
$\pi_{\theta}$, with parameters $\theta$, to control a mobile robot (e.g.,
multirotor). The policy needs to be capable of tracking a desired reference
trajectory given high-dimensional noisy sensor measurements, and has the form

$\mathbf{u}_{t}=\pi_{\theta}(\mathbf{o}_{t},\mathbf{X}^{\text{des}}_{t}),$ (1)

where $t$ denotes the discrete time index, $\mathbf{u}_{t}$ represents the
deterministic control actions, and
$\mathbf{o}_{t}=(\mathcal{I}_{t},\mathbf{o}_{\text{other},t})$ the high-
dimensional, noisy sensor measurements, comprised of an image
$\mathcal{I}_{t}$ captured by an onboard camera, and other noisy measurements
$\mathbf{o}_{\text{other,t}}$ (e.g., attitude, velocity). The $N+1$ steps of
the reference trajectory are written as
$\mathbf{X}^{\text{des}}_{t}=\\{\mathbf{x}^{\text{des}}_{0|t},\dots,\mathbf{x}^{\text{des}}_{N|t}\\}$,
where $\mathbf{x}^{\text{des}}_{i|t}$ indicates the desired state at the
future time $t+i$, as given at the current time $t$, and $N>0$ represents the
total number of given future desired states. Our objective is to efficiently
learn the policy parameters $\hat{\theta}^{*}$ by leveraging IL and
demonstrations provided by a model-based controller (expert).

System model. We assume available a model of the dynamics of the robot,
described by a set of linear (e.g., via linearization), discrete and time-
invariant equations:

$\mathbf{x}_{t+1}=\mathbf{A}\mathbf{x}_{t}+\mathbf{B}\mathbf{u}_{t}+\mathbf{w}_{t}\\\
$ (2)

where matrices $\mathbf{A}\in\mathfrak{R}^{n_{x}\times n_{x}}$ and
$\mathbf{B}\in\mathfrak{R}^{n_{x}\times n_{u}}$ represent the system dynamics,
$\mathbf{x}_{t}\in\mathbb{X}\subset\mathfrak{R}^{n_{x}}$ represents the state
of the system, and $\mathbf{u}_{t}\in\mathbb{U}\subset\mathfrak{R}^{n_{u}}$
represents the control inputs. The system is subject to state and inputs
constraint $\mathbb{X}$ and $\mathbb{U}$, assumed to be convex polytopes
containing the origin [31]. The quantity
$\mathbf{w}_{t}\in\mathbb{W}\subset\mathfrak{R}^{n_{x}}$ in (2) captures time-
varying additive process uncertainties. This includes disturbances and model
errors that the system may encounter at deployment time, or the ones caused by
the sim2real gap when collecting demonstrations using a simulator. It can
additionally capture disturbances encountered at deployment time that are not
present when collecting demonstrations from a real robot in a controlled
environment (e.g., lab2real gap). These uncertainties are assumed to belong to
a bounded set $\mathbb{W}$, assumed to be a polytope containing the origin
[31].

Sensing model. We assume that the model describing the measurement
$\bar{\mathbf{o}}_{t}\in\mathfrak{R}^{n_{o}}$ used by the expert is:

$\bar{\mathbf{o}}_{t}=\begin{bmatrix}\mathbf{o}_{\text{pos},t}\\\
\mathbf{o}_{\text{other},t}\\\
\end{bmatrix}=\mathbf{C}\mathbf{x}_{t}+\mathbf{v}_{t},$ (3)

where $\mathbf{C}\in\mathfrak{R}^{n_{x}\times n_{o}}$, and
$\mathbf{v}_{t}=[\mathbf{v}_{\text{\text{cam}},t}^{T},\mathbf{v}_{\text{other},t}^{T}]^{T}\in\mathbb{V}\subset\mathfrak{R}^{n_{o}}$
represents additive sensing uncertainties (e.g., noise, biases), sampled from
a bounded set $\mathbb{V}$. The expert has access to a vision-based position
estimator $g_{\text{cam}}$ that produces a noisy estimate
$\mathbf{o}_{\text{pos}}\in\mathfrak{R}^{3}$ of the position
$\bm{p}_{t}\in\mathfrak{R}^{3}$ of the robot from an image:

$\mathbf{o}_{\text{pos},t}=g_{\text{cam}}(\mathcal{I}_{t})=\bm{p}_{t}+\mathbf{v}_{\text{\text{cam}},t},$
(4)

where $\mathbf{v}_{\text{cam},t}$ is the associated noise/uncertainty.
$\mathbb{V}$ can be obtained via system identification, and/or prior knowledge
on the accuracy of $g_{\text{cam}}$.

State estimator model. We assume that the dynamics of the state estimator
employed by the expert during the demonstration collection phase can be
approximated by the linear (Luenberger) observer

$\hat{\mathbf{x}}_{t+1}=\mathbf{A}\hat{\mathbf{x}}_{t}+\mathbf{B}\mathbf{u}_{t}+\mathbf{L}(\bar{\mathbf{o}}_{t}-\hat{\mathbf{o}}_{t}),\quad\hat{\mathbf{o}}_{t}=\mathbf{C}\hat{\mathbf{x}}_{t},$
(5)

where $\hat{\mathbf{o}}_{t}$ is the predicted observation, and
$\hat{\mathbf{x}}_{t}\in\mathfrak{R}^{n_{x}}$ is the estimated state.
$\mathbf{L}\in\mathbb{R}^{n_{x}\times n_{o}}$ is the observer gain matrix, and
is chosen so that the matrix $\mathbf{A}-\mathbf{L}\mathbf{C}$ is Schur
stable. The observability index of the system $(\mathbf{A},\mathbf{C})$ is
assumed to be $1$, meaning that full state information can be retrieved from a
single noisy measurement. In this case, the observer plays the critical role
of filtering out the effects of noise and sensing uncertainties. Finally, we
assume that the state estimation dynamics and noise sensitivity of the learned
policy will approximately match the ones of the observer.

##  IV METHODOLOGY

The overall idea of our method is to generate trajectory tracking
demonstrations from noisy sensor measurements using an output feedback RTMPC
expert combined with a state estimator Eq. 5, leveraging the defined process
and sensing models. The chosen output feedback RTMPC framework is based on
[33], with its cost function modified to track a desired trajectory, and it is
summarized in Section IV-B. This framework provides:  a) robustdemonstrations,
taking into account the effect of process and sensing uncertainties, and  b) a
corresponding data augmentation strategy, key to overcome efficiency and
robustness challenges in IL.  Demonstrations can be collected via IL methods
such as BC [3] or DAgger [2]. Combined with the augmented data provided by our
strategy, the collected demonstrations are then used to train a policy Eq. 1
via supervised regression. Section V-A will tailor our methodology to control
a multirotor system.

###  IV-A Mathematical preliminaries

Set operations. Given the convex polytopes
$\mathbb{A}\subset\mathfrak{R}^{n},\mathbb{B}\subset\mathfrak{R}^{n}$ and the
linear mapping $\mathbf{C}\in\mathfrak{R}^{m\times n}$, we define:

  1. a)

Minkowski sum:
$\mathbb{A}\oplus\mathbb{B}\coloneqq\\{\mathbf{a}+\mathbf{b}\in\mathfrak{R}^{n}\>|\>\mathbf{a}\in\mathbb{A},\>\mathbf{b}\in\mathbb{B}\\}$

  2. b)

Pontryagin diff.:
$\mathbb{A}\ominus\mathbb{B}\coloneqq\\{\mathbf{c}\in\mathfrak{R}^{n}\>|\>\mathbf{c+b}\in\mathbb{A},\forall\mathbf{b}\in\mathbb{B}\\}$

Robust Positive Invariant (RPI) set. Given a closed-loop Schur-stable system
$\mathbf{A}\in\mathfrak{R}^{n\times n}$ subject to the uncertainty
$\mathbf{d}_{t}\in\mathbb{D}\subset\mathfrak{R}^{n}$:
$\bm{\xi}_{t+1}=\mathbf{A}\bm{\xi}_{t}+\mathbf{d}_{t},$ we define the minimal
Robust Positive Invariant (RPI) set $\mathbb{S}$ as the smallest set
satisfying the following property:

$\bm{\xi}_{0}\in\mathbb{S}\implies\bm{\xi}_{t}\in\mathbb{S},\;\forall\;\mathbf{d}_{t}\in\mathbb{D},\;t>0.$
(6)

This means that if the initial state $\bm{\xi}_{0}$ of the system starts in
the RPI set $\mathbb{S}$, then it will never escape from such set, regardless
of the realizations $\mathbf{d}_{t}$ of the disturbances in $\mathbb{D}$.

###  IV-B Output feedback robust tube MPC for trajectory tracking

The objective of the output feedback RTMPC for trajectory tracking is to
regulate the system in Eq. 2 along a given desired trajectory
$\mathbf{X}^{\text{des}}_{t}$, while ensuring satisfaction of the state and
actuation constraints $\mathbb{X},\mathbb{U}$ regardless of the process noise
$\mathbf{w}$ and sensing noise $\mathbf{v}$ in Eq. 2 and Eq. 3, respectively.

Optimization problem. To fulfill these requirements, the RTMPC computes at
every timestep $t$ a sequence of “safe” reference states
$\mathbf{\bar{X}}_{t}=\\{\bar{\mathbf{x}}_{0|t},\dots,\bar{\mathbf{x}}_{N|t}\\}$
and actions
$\mathbf{\bar{U}}_{t}=\\{\bar{\mathbf{u}}_{0|t},\dots,\bar{\mathbf{u}}_{N-1|t}\\}$
along a planning horizon of length $N+1$. This is achieved by solving the
optimization problem:

$\begin{split}\mathbf{\bar{U}}_{t}^{*},\mathbf{\bar{X}}_{t}^{*}=\underset{\mathbf{\bar{U}}_{t},\mathbf{\bar{X}}_{t}}{\text{argmin}}&\|\mathbf{e}_{N|t}\|^{2}_{\mathbf{P}}+\sum_{i=0}^{N-1}\|\mathbf{e}_{i|t}\|^{2}_{\mathbf{Q}}+\|\mathbf{u}_{i|t}\|^{2}_{\mathbf{R}}\\\
\text{subject
to}\>\>&\bar{\mathbf{x}}_{i+1|t}=\mathbf{A}\bar{\mathbf{x}}_{i|t}+\mathbf{B}\bar{\mathbf{u}}_{i|t},\\\
&\bar{\mathbf{x}}_{i|t}\in\bar{\mathbb{X}},\>\>\bar{\mathbf{u}}_{i|t}\in\bar{\mathbb{U}},\\\
&\mathbf{\bar{x}}_{N|t}\in\mathbb{X}_{N},\>\>\hat{\mathbf{x}}_{t}\in\mathbb{Z}\oplus\bar{\mathbf{x}}_{0|t},\\\
\end{split}$ (7)

where $\mathbf{e}_{i|t}=\bar{\mathbf{x}}_{i|t}-\mathbf{x}^{\text{des}}_{i|t}$
represents the trajectory tracking error. The positive definite matrices
$\mathbf{Q}$ (size $n_{x}\times n_{x}$) and $\mathbf{R}$ (size $n_{u}\times
n_{u}$) are tuning parameters, and define the trade-off between deviations
from the desired trajectory and actuation usage. A terminal cost
$\|\mathbf{e}_{N|t}\|^{2}_{\mathbf{P}}$ and terminal state constraint
$\mathbf{\bar{x}}_{N|t}\in\bar{\mathbb{X}}_{N}$ are introduced to ensure
stability [33], where $\mathbf{P}$ (size $n_{x}\times n_{x}$) is a positive
definite matrix, obtained by solving the infinite horizon optimal control LQR
problem, using the system dynamics $\mathbf{A}$, $\mathbf{B}$ in Eq. 2 and the
weights $\mathbf{Q}$ and $\mathbf{R}$. The “safe” references
$\mathbf{\bar{X}}_{t}$, $\mathbf{\bar{U}}_{t}$ are generated based on the
nominal (e.g., not perturbed) dynamics, via the constraints
$\bar{\mathbf{x}}_{i+1|t}=\mathbf{A}\bar{\mathbf{x}}_{i|t}+\mathbf{B}\bar{\mathbf{u}}_{i|t}$.

Ancillary controller. The control input for the real system is obtained via a
feedback controller, called ancillary controller:

$\mathbf{u}_{t}=\mathbf{\bar{\mathbf{u}}}^{*}_{t}+\mathbf{K}(\hat{\mathbf{x}}_{t}-\bar{\mathbf{x}}^{*}_{t}),$
(8)

where $\bar{\mathbf{u}}^{*}_{0|t}=\bar{\mathbf{u}}^{*}_{t}$ and
$\bar{\mathbf{x}}^{*}_{0|t}=\bar{\mathbf{x}}^{*}_{t}$. The matrix $\mathbf{K}$
is computed by solving the LQR problem with $\mathbf{A}$, $\mathbf{B}$,
$\mathbf{Q}$, $\mathbf{R}$. This controller ensures that the system remains
inside a set $\mathbb{Z}$ (“cross-section” of a tube) centered around
$\bar{\mathbf{x}}_{t}^{*}$ regardless of the realization of process and
sensing uncertainty, provided that the state of the system starts in such tube
(constraint $\hat{\mathbf{x}}_{t}\in\mathbb{Z}\oplus\bar{\mathbf{x}}_{0|t}$).

Tube and robust constraint satisfaction. Process and sensing uncertainties are
taken into account by tightening the constraints $\mathbb{X}$, $\mathbb{U}$ by
an amount which depends on $\mathbb{Z}$, obtaining $\bar{\mathbb{X}}$ and
$\bar{\mathbb{U}}$.

We start by describing how to compute the set $\mathbb{Z}$, which depends on
the errors caused by uncertainties when employing the ancillary controller Eq.
8 and the state estimator Eq. 5. The system considered in Eq. 2 and Eq. 3 is
subject to sensing $\mathbb{V}$ and process $\mathbb{W}$ uncertainties,
sources of two errors:  a) the state estimation error
$\bm{\xi}^{\text{est}}_{t}=\mathbf{x}_{t}-\hat{\mathbf{x}}_{t}$, which
captures the deviations of the state estimate $\hat{\mathbf{x}}_{t}$ from the
true state $\mathbf{x}_{t}$ of the system;  b) the control error
$\bm{\xi}^{\text{ctrl}}_{t}=\hat{\mathbf{x}}_{t}-\bar{\mathbf{x}}_{t}^{*}$,
which captures instead deviations of $\hat{\mathbf{x}}_{t}$ from the reference
plan $\bar{\mathbf{x}}_{t}^{*}=\bar{\mathbf{x}}^{*}_{0|t}$.  These errors can
be combined in a vector
$\bm{\xi}_{t}=[{\bm{\xi}^{\text{est}}_{t}}^{T},{\bm{\xi}^{\text{ctrl}}_{t}}^{T}]^{T}$
whose dynamics evolve according to (see [32, 33]):

$\bm{\xi}_{t+1}=\mathbf{A}_{\xi}\bm{\xi}_{t}+\bm{\delta}_{t},\;\bm{\delta}_{t}\in\mathbb{D}\\\
$ (9)
$\mathbf{A}_{\xi}=\begin{bmatrix}\mathbf{A}-\mathbf{L}\mathbf{C}&\mathbf{0}_{n_{x}}\\\
\mathbf{L}\mathbf{C}&\mathbf{A}+\mathbf{B}\mathbf{K}\end{bmatrix},\quad\mathbb{D}=\begin{bmatrix}\mathbf{I}_{n_{x}}&-\mathbf{L}\\\
\mathbf{0}_{n_{x}}&\mathbf{L}\end{bmatrix}\begin{bmatrix}\mathbb{W}\\\
\mathbb{V}\end{bmatrix}.$

We observe that, by design, $\mathbf{A}_{\xi}$ is Schur-stable, and
$\mathbb{D}$ is a convex polytope. Then, the possible set of state estimation
and control errors correspond to the minimal RPI set $\mathbb{S}$ associated
with the system $\mathbf{A}_{\xi}$ under the uncertainty set $\mathbb{D}$.

We now introduce the error between the true state $\mathbf{x}_{t}$ and the
reference state $\bar{\mathbf{x}}_{t}^{*}$:
$\bm{\xi}^{\text{tot}}=\mathbf{x}_{t}-\bar{\mathbf{x}}_{t}^{*}=\bm{\xi}^{\text{ctrl}}+\bm{\xi}^{\text{est}}.$
As a consequence, the set (tube) $\mathbb{Z}$ that describes the possible
deviations of the true state $\mathbf{x}_{t}$ from the reference
$\bar{\mathbf{x}}_{t}^{*}$ is:

$\mathbb{Z}=\begin{bmatrix}\bm{I}_{n_{x}}&\bm{I}_{n_{x}}\\\
\end{bmatrix}\mathbb{S}.$ (10)

Finally, the effects of noise and uncertainties can be taken into account by
tightening the constraints of an amount:

$\bar{\mathbb{X}}\coloneqq\mathbb{X}\ominus\mathbb{Z},\quad\bar{\mathbb{U}}\coloneqq\mathbb{U}\ominus\begin{bmatrix}\bm{0}_{n_{x}}&\mathbf{K}\end{bmatrix}\mathbb{S}.$
(11)

###  IV-C Tube-guided data augmentation for visuomotor learning

Imitation learning objective. We assume that the given output feedback RTMPC
controller in Eq. 7, Eq. 8 and the state observer in Eq. 3 can be represented
via a policy $\pi_{\theta^{*}}$ [34], where $\pi$ is of the form in Eq. 1,
with parameters $\theta^{*}$. Our objective is to design an IL and data
augmentation strategy to efficiently learn the parameters $\hat{\theta}^{*}$
for the sensorimotor policy Eq. 1, collecting demonstrations from the expert.
Using IL, this objective is achieved by minimizing the expected value of a
distance metric $J(\theta,\theta^{*}|\tau)$ over a distribution of
trajectories $\tau$:

$\hat{\theta}^{*}=\text{arg}\min_{\theta}\mathbb{E}_{p(\tau|\pi_{\theta},\mathcal{K}_{\text{target}})}[J(\theta,\theta^{*}|\tau)],$
(12)

where $J(\theta,\theta^{*}|\tau)$ captures the differences between the
trajectories generated by the expert $\pi_{\theta^{*}}$ and the trajectories
produced by the learner $\pi_{\theta}$. The variable
$\tau:=((\mathbf{o}_{0},\mathbf{u}_{0},\mathbf{X}^{\text{des}}_{0}),\dots,(\mathbf{o}_{T},\mathbf{u}_{T},\mathbf{X}^{\text{des}}_{T}))$
denotes a $T+1$ step (observation, action, reference) trajectory sampled from
the distribution $p(\tau|\pi_{\theta},\mathcal{K}_{\text{target}})$. Such
distribution represents all the possible trajectories induced by the learner
policy $\pi_{\theta}$ in the deployment environment
$\mathcal{K}_{\text{target}}$, under all the possible instances of the process
and sensing uncertainties. In this work, the chosen distance metric is the MSE
loss:

$J(\theta,\theta^{*}|\tau)=\frac{1}{T}\sum_{t=0}^{T-1}\|\pi_{\theta}(\mathbf{o}_{t},\mathbf{X}_{t}^{\text{des}})-\pi_{\theta^{*}}(\mathbf{o}_{t},\mathbf{X}_{t}^{\text{des}})\|_{2}^{2}.$
(13)

As observed in [6], the presence of uncertainties in the deployment domain
$\mathcal{K}_{\text{target}}$ makes the setup challenging, as demonstrations
are usually collected in a training environment $\mathcal{K}_{\text{source}}$
that presents only a subset of all the possible uncertainty realizations. At
deployment time, uncertainties may drive the learned policy towards the region
of the input space for which no demonstrations have been provided (covariate
shift), with potentially catastrophic consequences.

Tube and ancillary controller for data augmentation. To overcome these
limitations, we design a data augmentation strategy that takes into account
and compensates for the effects of process and sensing uncertainties
encountered in $\mathcal{K}_{\text{target}}$, and that it is capable of
producing the augmented input-output data needed to learn the sensorimotor
policy Eq. 1. We do so by extending our previous approach [6], named Sampling
Augmentation (SA), which provides a strategy to efficiently learn a policy
robust to process uncertainty. SA relies on recognizing that the tube
$\mathbb{Z}$, as computed via a RTMPC, represents a model of the states that
the system may visit when subject to process uncertainties. The ancillary
controller Eq. 8 provides a computationally efficient way to generate the
actions to take inside the tube, as it guarantees that the system remains
inside the tube for every possible realization of the process uncertainty.

Our new approach, named Visuomotor Sampling Augmentation (VSA), employs the
output feedback variant of RTMPC in Section IV-B, as its tube represents a
model of the states that might be visited by the system when subject to
process and sensing uncertainty, and its ancillary controller provides a way
to efficiently compute corresponding actions, guaranteeing that the system
remains inside the tube $\mathbb{Z}\oplus\bar{\mathbf{x}}^{*}_{t}$ in Eq. 10
for every possible realization of $\mathbf{w}_{t}\in\mathbb{W}$ and
$\mathbf{v}_{t}\in\mathbb{V}$. These properties can be leveraged to design a
data augmentation strategy while collecting demonstrations in the source
domain $\mathcal{K}_{\text{source}}$.

More specifically, let $\tau^{\text{aux}}$ be a $T+1$ step demonstration
generated by the output feedback RTMPC in Section IV-B, collected in the
training domain $\mathcal{K}_{\text{source}}$:

$\tau^{\text{aux}}=((\mathbf{o}_{0},\mathbf{u}_{0},(\bar{\mathbf{u}}_{0}^{*},\bar{\mathbf{x}}_{0}^{*}),\mathbf{X}^{\text{des}}_{0}),\dots{\\\
}\dots,(\mathbf{o}_{T},\mathbf{u}_{T},(\bar{\mathbf{u}}_{T}^{*},\bar{\mathbf{x}}_{T}^{*}),\mathbf{X}^{\text{des}}_{T})).$
(14)

Then, at every timestep $t$ in the collected demonstration
$\tau^{\text{aux}}$, we can compute extra (state, action) pairs
$(\mathbf{x}^{+},\mathbf{u}^{+})$ by sampling extra states from inside the
tube $\mathbf{x}^{+}\in\bar{\mathbf{x}}_{t}^{*}\oplus\mathbb{Z}$, and the
corresponding robust control action $\mathbf{u}^{+}$:

$\mathbf{u}^{+}=\mathbf{\bar{\mathbf{u}}}_{t}^{*}+\mathbf{K}(\mathbf{x}^{+}-\bar{\mathbf{x}}_{t}^{*}).$
(15)

Augmented sensory data generation. The output feedback RTMPC-based
augmentation strategy described so far provides a way to generate extra
(state, action) samples for data augmentation, but not the sensor observations
$\mathbf{o}^{+}=(\mathcal{I}^{+},\mathbf{o}_{\text{other}}^{+})$ required as
input of the sensorimotor policy Eq. 1. We overcome this issue by employing
observation models Eq. 3 available for the expert, obtaining a way to generate
extra observations $\mathbf{o}^{+}$, given extra states $\mathbf{x}^{+}$.

In the context of learning the visuomotor policy Eq. 1, we generate synthetic
camera images $\mathcal{I}^{+}$ via an inverse pose estimator
$g^{-1}_{\text{\text{cam}}}$, mapping camera poses $\mathbf{T}_{IC}$ to image
observations $\mathcal{I}$

$\mathcal{I}^{+}=g_{\text{\text{cam}}}^{-1}(\mathbf{T}_{IC}^{+})$ (16)

where $\mathbf{T}_{IC}$ denotes a homogeneous transformation matrix [35] from
a world (inertial) frame $I$ to a camera frame $C$. The inverse camera model
$g^{-1}_{\text{\text{cam}}}$ can be obtained by generating a 3D reconstruction
of the environment from the sequence of observations obtained in the collected
demonstrations, and by modeling the intrinsic and extrinsic parameters of the
onboard camera used by the robot [35]. Extra camera poses
$\textbf{T}_{IC}^{+}$ can be obtained from the extra state vector
$\mathbf{x}^{+}$, assuming that the state contains position and orientation
information

$\textbf{T}_{IC}^{+}=\textbf{T}_{IB}(\mathbf{x}^{+})\textbf{T}_{BC},$ (17)

where $\mathbf{T}_{IB}$ represents the homogeneous transformation expressing
the pose of a robot-fixed body frame $B$ with respect to $I$.
$\mathbf{T}_{BC}$ describes a known rigid transformation between the body of
the robot $B$ and the optical frame of the camera $C$. The full extra
observation $\mathbf{o}^{+}$ can be obtained by additionally computing
$\mathbf{o}_{\text{other}}^{+}$ via the measurement model Eq. 3, assuming zero
noise:

$\mathbf{o}^{+}=(\mathcal{I}^{+},\mathbf{o}_{\text{other}}^{+}),\;\;\mathbf{o}_{\text{other}}^{+}=\mathbf{S}\mathbf{C}\mathbf{x}^{+},$
(18)

where $\mathbf{S}$ is a selection matrix.

##  V EVALUATION

###  V-A Visuomotor trajectory tracking on a multirotor

Task setup. We consider the task of learning a visuomotor trajectory tracking
policy for a multirotor, starting from slightly different initial states. The
chosen trajectory is an eight-shape (lemniscate), with a velocity of up to
$3.5$ m/s and a total duration of $30.0$ s. We assume that the robot is
equipped with a downward-facing monocular camera, and it has available onboard
noisy velocity ${}_{I}\bm{v}\in\mathfrak{R}^{3}$ and roll ${}_{I}\varphi$,
pitch ${}_{I}\vartheta$ information, corrupted by measurement noise
$\mathbf{v}_{\text{other},t}$:

$\mathbf{o}_{\text{other},t}=[_{I}\bm{v}^{T},_{I}\varphi,_{I}\vartheta]^{T}+\mathbf{v}_{\text{other},t}.$
(19)

$I$ denotes a yaw-fixed, gravity aligned inertial reference frame in which the
quantities are expressed, and $\mathbf{v}_{\text{other}}$ is a zero-mean,
Gaussian noise with three standard deviations
$3\bm{\sigma}_{\text{other}}=[0.2,0.2,0.2,0.05,0.05]^{T}$ (units in $m/s$ for
velocity, and $rad$ for tilt). Information on the position
${}_{I}\bm{p}\in\mathfrak{R}^{3}$ of the robot is not provided to the learned
policy, and it must be inferred from the onboard camera. The output feedback
RTMPC has instead additionally access to noisy position information corrupted
by additive, zero mean Gaussian noise $\mathbf{v}_{\text{cam}}$ with three
standard deviations $3\bm{\sigma}_{\text{cam}}=[0.2,0.2,0.4]^{T}$ ($m$).
Demonstrations are collected from the output feedback RTMPC expert in a
nominal domain $\mathcal{K}_{\text{source}}$ that presents sensing noise and
model uncertainties (e.g., due to drag, unmodeled attitude dynamics). We
consider two deployment environments $\mathcal{K}_{\text{target}}$, one with
sensing noise, matching $\mathcal{K}_{\text{source}}$, and one that
additionally presents wind-like force disturbances $\mathbf{f}_{\text{wind}}$,
with magnitude corresponding to $15\%$-$20\%$ of the weight of the multirotor
($12.75$ N). During training and deployment, the robot should not collide with
the walls/ceiling nor violate additional safety limits (maximum velocity,
roll, pitch).

Output feedback RTMPC design. To design the output feedback RTMPC, we employ a
hover-linearized model of a multirotor[36]. The state of the robot ($n_{x}=8$)
is:

$\mathbf{x}=[_{I}\bm{p}^{T},_{I}\bm{v}^{T},_{I}\varphi,_{I}\vartheta]^{T},$
(20)

while the output ($n_{u}=3$) $\mathbf{u}_{t}$ corresponds to roll, pitch and
thrust commands, which a cascaded attitude controller executes. Safety limits
and position limits are encoded as state constraints. The sampling period for
the controller and prediction model is set to $T_{s}=0.1$ s, and the
prediction horizon is $N=30$, ($3.0$ s). The tube $\mathbb{Z}$ is computed
assuming the observation model in Eq. 3 to be:
$\bar{\mathbf{o}}_{t}=\mathbf{x}_{t}+\mathbf{v}_{t},$ where the bounded
measurement uncertainty matches the three standard deviations of the noise
encountered in the environment:
$\mathbb{V}=\\{\mathbf{v}\;|\;\|\mathbf{v}\|_{\infty}\leq
3[\bm{\sigma}_{\text{cam}}^{T},\bm{\sigma}_{\text{other}}^{T}]^{T}\\}$. The
observer gain matrix $\mathbf{L}$ is computed by assuming fast state
estimation dynamics, by placing the poles of the estimation error dynamics
$\mathbf{A-LC}$ at $30.0$ rad/s.

Figure 2: Left: 3D reconstruction of the training environment employed to
generate data augmentation. The camera images used to generate the
reconstruction are obtained from a single demonstration collected with the
real robot. Right: Images generated by the simulated onboard camera along the
considered trajectory in the 3D mesh of the environment. If the camera is
pointing at regions where the reconstruction is not available (e.g., top and
bottom right images), the area of missing data is left featureless (white),
without providing any additional information useful for position estimation.

The process uncertainty $\mathbb{W}$ is set to correspond to a bounded force-
like disturbance of magnitude equal to $25\%$ of the weight of the multirotor,
capturing the physical limits of the platform. Using these priors on
uncertainties, sensing noise and the system Eq. 9, we compute an approximation
of the tube $\mathbb{Z}$ via Monte Carlo simulations, randomly sampling
instances of the uncertainties. The RPI set $\mathbb{S}$ is obtained by
computing an axis-aligned bounding box approximation of the error
trajectories.

Policy architecture. The architecture of the learned policy Eq. 1 is
represented in Figure 3. The policy takes as input the raw image produced by
the onboard camera, the desired trajectory, and noisy attitude and velocity
information $\mathbf{o}_{\text{other}}$. It employs three convolutional
layers, which map the raw images into a lower-dimensional feature space; these
features are then combined with the additional velocity and attitude inputs,
and passed through a series of fully connected layers. The generated action
$\mathbf{u}_{t}$ is then clipped such that $\mathbf{u}_{t}\in\mathbb{U}$. To
promote learning of internal features relevant to estimating the robot’s
state, the output of the policy is augmented to predict the current state
$\hat{\mathbf{x}}$ (or $\mathbf{x}^{+}$ for the augmented data), modifying Eq.
13 accordingly. This output is not used at deployment time, but it was found
to improve the performance.

Figure 3: Architecture of the employed visuomotor policy. The policy takes as
input a raw camera image, a desired trajectory
$\mathbf{x}^{\text{des}}_{0|t},\dots,\mathbf{x}^{\text{des}}_{N|t}$ and noisy
measurements of the velocity $\bm{v}_{t}$ and tilt (roll $\varphi_{t}$, pitch
$\vartheta_{t}$) of the multirotor. It outputs an action $\textbf{u}_{t}$,
representing a desired roll, pitch and thrust set-points for the cascaded
attitude controller. The policy additionally outputs an estimate of the state
$\hat{\mathbf{x}}_{t}$, which was found useful to promote learning of features
relevant for position estimation.

###  V-B Dense reconstruction of the environment

We generate a 3D mesh of the environment needed for data augmentation via VSA
from a sequence of real images collected by the onboard downward-facing camera
of the Qualcomm Snapdragon Flight Pro [37] board mounted on a multirotor. The
images are obtained by executing the trajectory that we intend to learn, which
helps us validate the assumption that it is possible to collect a quantity and
quality of images sufficient to create an accurate 3D mesh from a single
demonstration. Overall we employ $145$ black and white images of size
$640\times 480$ pixels. The 3D reconstruction of the considered indoor flight
space and the resulting mesh, shown in Figure 2 (left), are generated via the
3D photogrammetry pipeline provided by Meshroom [38]. The scale of the
generated reconstruction, which is unobservable from the monocular images, is
set manually via prior knowledge of the dimensions of the environment. The
obtained mesh is then integrated with our multirotor simulator. We
additionally simulate a pinhole camera, whose extrinsic and intrinsic are set
to approximately match the parameters of the real onboard camera. Figure 2
(right) shows examples of synthetic images from our simulator.

###  V-C Choice of baselines, training setup and hyperparameters

Demonstration collection, training and evaluation is performed in simulation,
employing a realistic full nonlinear multirotor model (e.g., [36]). We apply
our data augmentation strategy, VSA, to BC and DAgger, comparing their
performance without any data augmentation. We additionally combine BC and
DAgger with Domain Randomization (DR); in this case, during demonstration
collection, we apply wind disturbances with magnitude sampled from
$10\%$-$110\%$ of the maximum wind force at deployment. We set $\beta$,
hyperparameter of DAgger controlling the probability of using actions from the
expert instead of the learner policy, to be $\beta=1$ for the first set of
demonstrations, and $\beta=0$ otherwise. For every method, we perform our
analysis in $M$ iterations, where at every $m$th iteration we:  (i) collect
$K$ new demonstrations via the output feedback RTMPC expert and the state
estimator;  (ii) train a policy with randomly initialized weights using all
the demonstrations collected so far, and  (iii) evaluate the obtained policy
for $20$ times, under different realizations of the uncertainty and initial
states of the robot.  We record the time taken to perform phase (i)-(ii) at
the $m$th iteration, denoted as $T_{m}^{\text{iter}}$. The entire analysis is
repeated across $10$ random seeds. We use $M=2$, $K=1$ for methods using VSA,
while $M=15$, $K=10$ for all the other methods, in order to speed up their
computation. When using VSA, we additionally train the policy incrementally,
i.e., we only use the newly collected demonstrations and the corresponding
augmented data to update the previously trained policy. All the policies are
trained for $50$ epochs using the ADAM optimizer, with a learning rate of
$0.001$ and a batch size of $32$. When using VSA, we augment the training data
by generating a number $S=\\{50,100,200\\}$ of observation-action samples, for
every timestep, by uniformly sampling states inside the tube. We denote the
corresponding method as VSA$-S$.

###  V-D Efficiency, robustness and performance

Figure 4: Robustness in the deployment domains under measurement uncertainties (sensing noise) and process noise (wind-like disturbances) as a function of the number of demonstrations. The shaded area represent the $95\%$ confidence intervals. Our approach, Visuomotor Sampling Augmentation (VSA)-$200$ (where $200$ indicates the number of samples extracted from the tube per timestep), after a single demonstration achieves full robustness in the environment w/o wind, and $>95\%$ robustness in the environment with wind. Evaluation performed across $10$ random seeds, with $20$ trajectories per seed, starting from different initial states. The lines of the VSA-based approaches overlap. Figure 5: Robustness as a function of the training time, which includes the time to collect demonstrations from the expert and to train the policy. Robustness is considered under only measurement uncertainties (sensing noise) and in combination with process noise (wind-like disturbances). This result highlights that our approach, Visuomotor Sampling Augmentation (VSA), is capable to achieve $>95\%$ robustness under sensing and process noise in less than half of the time than the best performing baseline approaches (DAgger+DR). Reducing the number of samples extracted from the tube (VSA-{50, 100, 200}) significantly reduces the training time, with a minimal change in robustness. TABLE I: Properties of the considered visuomotor policy learning approaches, including robustness, performance, and demonstration efficiency. We define an approach easy if it does not require applying disturbances during the demonstration collection phase, and it is safe if it does not violate state constraints (e.g., wall collision) during training. Performance captures the relative error between the trajectory generated by output feedback RTMPC (expert) and the learned policy. Demonstration efficiency indicates the number of demonstrations required for one approach to achieve at least $90\%$ success rate. Performance and robustness of the baseline approaches are evaluated after $150$ demonstrations, while VSA methods are evaluated after only $2$ demonstrations. Method | Training |  Robustness succ. rate (%, $\uparrow$) |  Performance expert gap (%, $\downarrow$) |  Demonstration Efficiency ($\downarrow$)
---|---|---|---|---
Robustif. |  Imitation |  Easy |  Safe |  Noise |  Noise, Wind |  Noise |  Noise, Wind |  Noise |  Noise, Wind
- |  BC |  Yes |  Yes |  59.3 |  0.8 |  98.4 |  545.7 |  - |  -
DAgger |  Yes |  No |  100.0 |  17.0 |  8.3 |  295.2 |  50 |  -
DR |  BC |  No |  Yes |  55.3 |  9.3 |  128.0 |  186.7 |  - |  -
DAgger |  No |  No |  100.0 |  91.0 |  5.4 |  27.3 |  40 |  120
VSA-50 |  BC |  Yes |  Yes |  100.0 |  97.0 |  27.4 |  24.3 |  1 |  1
DAgger |  Yes |  Yes |  100.0 |  97.0 |  13.6 |  17.5 |  1 |  1
VSA-100 |  BC |  Yes |  Yes |  100.0 |  97.5 |  11.5 |  14.1 |  1 |  1
DAgger |  Yes |  Yes |  100.0 |  94.5 |  8.9 |  13.0 |  1 |  1
VSA-200 |  BC |  Yes |  Yes |  100.0 |  97.5 |  12.7 |  17.9 |  1 |  1
DAgger |  Yes |  Yes |  100.0 |  99.5 |  4.5 |  14.9 |  1 |  1

Robustness and efficiency. We evaluate the robustness of the considered
methods by monitoring the success rate, as the percentage of episodes where
the robot never violates any state constraint in the considered deployment
environment. This metric is evaluated as a function of the number of
demonstrations collected in the source environment. The results are shown in
Figure 4 and Table I, and highlight that all VSA methods, combined with either
DAgger or BC, can achieve complete robustness in the environment without wind
after a single demonstration, and at least $94.5\%$ success rate in the
environment with wind (DAgger+VSA-$50$). A second demonstration can provide
$>99\%$ robustness even in the environment with wind (DAgger+VSA-$200$). The
baseline approaches require instead $40$-$50$ demonstrations to achieve full
success rate in the environment without wind, and the best performing baseline
(DAgger+DR) requires $120$ demonstrations to achieve its top robustness
($97.3\%$ success rate) in the more challenging environment. We additionally
evaluate the computational efficiency as the time (training time) required to
achieve a certain success rate. This is done by monitoring the robustness
achieved at the $m$th iteration as a function of the time to reach that
iteration, $\sum_{i=0}^{m}T^{\text{iter}}_{i}$. The results in Figure 5
highlight that our approach requires less than half training time than the
best performing baseline (DAgger+DR) to achieve higher or comparable
robustness in the most challenging environment. Additionally, reducing the
number of samples in VSA can enable large training time gains with a minimal
loss in robustness.

(a) With sensing noise, no wind.

(b) With sensing noise and wind gusts.

Figure 6: Executed trajectory and velocity of the multirotor in our simulated environment under sensing noise and challenging wind-like disturbances. The policy has been trained from a single demonstration using DAgger+VSA-100. The wind disturbances applied are force steps, representing wind gusts, ranging $15\%$ to $20\%$ of the weight of the robot, and varying in magnitude and direction every $4.0$ s. TABLE II: Trajectory tracking errors. The position error is expressed in $m^{2}$, while velocity error in $m^{2}/s^{2}$. | DAgger+VSA-$100$ | Expert
---|---|---
Noise | Noise, Wind | Noise | Noise, Wind
MSE | Pos. | 0.332 | 0.593 | 0.256 | 0.562
Vel. | 0.579 | 0.626 | 0.563 | 0.598

Performance at convergence. We evaluate the performance of the considered
approaches via the expert gap, capturing the relative error with respect to
the output feedback RTMPC expert of the stage cost
$\sum_{t}\mathbf{\|}\mathbf{e}_{t}\|_{\mathbf{Q}}^{2}+\|\mathbf{u}_{t}\|^{2}_{\mathbf{R}}$
along the trajectories executed in the two considered deployment environment.
We exclude from the performance evaluation those trajectories that violate
state constraints (not robust). Table I reports the expert gap at the
convergence of the various methods. It highlights that DAgger combined with
VSA (DAgger+VSA) performs comparably or better than the most robust baseline
(DAgger+DR) but at a fraction of the number of demonstrations ($2$ instead of
$150$) and training time. Figure 6 shows a qualitative evaluation of the
trajectory tracked by the learned policy and highlights that the robot remains
close to the reference despite the relatively high velocity, wind, and sensing
noise. Table II gives the corresponding MSE for the position and velocity,
showing that the obtained visuomotor policy has tracking errors comparable to
those of the output feedback RTMPC expert, despite using vision information to
infer the position of the robot, while the expert has access to noisy ground
truth position information.

Efficiency at deployment. Table III reports the time to compute a new action.
It shows that on average the compressed policy is $17.6$ times faster, even
when excluding the cost of state estimation, than the corresponding model-
based controller. Evaluation performed on an Intel i9-10920 ($12$ cores) with
two Nvidia RTX 3090 GPUs.

TABLE III: Computation time required to generated a new action for the output feedback RTMPC (Expert) and the proposed learned visuomotor neural network policy (Policy), given a sensory observation. The learned neural network policy is $17.6$ times faster than the optimization-based expert. | Time (ms)
---|---
Method |  Setup |  Mean |  St. Dev. |  Min |  Max
Expert |  CVXPY [39] |  $15.70$ |  $13.37$ |  $7.37$ |  $76.56$
Policy |  PyTorch |  $\mathbf{0.89}$ |  $\mathbf{0.03}$ |  $\mathbf{0.84}$ |  $\mathbf{1.02}$

##  VI CONCLUSIONS

We have presented a strategy for efficient and robust sensorimotor policy
learning. Our key idea relies on the co-design of controller and data
augmentation strategy with IL methods. Such controller and data augmentation
approach leverage the theory of output feedback RTMPC to compute relevant
observations and actions for data augmentation—considering the effects of
sensing noise, domain uncertainties, and disturbances. We have tailored our
approach to the context of visuomotor policy learning, relying on a mesh of
the considered environment to generate tube-guided augmented views. Our
numerical evaluation has shown that the proposed method can learn a robust
visuomotor policy from a few demonstrations, outperforming IL baselines in
demonstration and computational efficiency. Future works will focus on
robustifying the policy to uncertainties/imperfections in the 3D mesh of the
environment ($g_{\text{cam}}^{-1}$), accounting for state estimation dynamics
in the data augmentation procedure, and performing hardware experiments.

## ACKNOWLEDGMENT

This work was funded by the Air Force Office of Scientific Research MURI
FA9550-19-1-0386.

## References

  * [1] B. D. Argall, S. Chernova, M. Veloso, and B. Browning, “A survey of robot learning from demonstration,” _Robotics and autonomous systems_ , vol. 57, no. 5, pp. 469–483, 2009.
  * [2] S. Ross, G. Gordon, and D. Bagnell, “A reduction of imitation learning and structured prediction to no-regret online learning,” in _Proceedings of the fourteenth international conference on artificial intelligence and statistics_. JMLR Workshop and Conference Proceedings, 2011, pp. 627–635.
  * [3] D. A. Pomerleau, “Alvinn: An autonomous land vehicle in a neural network,” Carnegie-Mellon Univ Pittsburgh PA Artificial Intelligence and Psychology, Tech. Rep., 1989.
  * [4] D. Scaramuzza and F. Fraundorfer, “Visual odometry [tutorial],” _IEEE Robotics Automation Magazine_ , vol. 18, no. 4, pp. 80–92, 2011.
  * [5] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in _Robotics: Science and Systems_ , vol. 2, no. 9. Berkeley, CA, 2014, pp. 1–9.
  * [6] A. Tagliabue, D.-K. Kim, M. Everett, and J. P. How, “Demonstration-efficient guided policy search via imitation of robust tube mpc,” in _2022 International Conference on Robotics and Automation (ICRA)_ , 2022, pp. 462–468.
  * [7] A. Loquercio, E. Kaufmann, R. Ranftl, A. Dosovitskiy, V. Koltun, and D. Scaramuzza, “Deep drone racing: From simulation to reality with domain randomization,” _IEEE Transactions on Robotics_ , vol. 36, no. 1, pp. 1–14, 2019.
  * [8] A. Loquercio, E. Kaufmann, R. Ranftl, M. Müller, V. Koltun, and D. Scaramuzza, “Learning high-speed flight in the wild,” _Science Robotics_ , vol. 6, no. 59, p. eabg5810, 2021.
  * [9] T. Zhang, G. Kahn, S. Levine, and P. Abbeel, “Learning deep control policies for autonomous aerial vehicles with mpc-guided policy search,” in _2016 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2016, pp. 528–535.
  * [10] G. Kahn, T. Zhang, S. Levine, and P. Abbeel, “Plato: Policy learning using adaptive trajectory optimization,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 3342–3349.
  * [11] E. Kaufmann, A. Loquercio, R. Ranftl, M. Müller, V. Koltun, and D. Scaramuzza, “Deep drone acrobatics,” _arXiv preprint arXiv:2006.05768_ , 2020.
  * [12] S. Ross, N. Melik-Barkhudarov, K. S. Shankar, A. Wendel, D. Dey, J. A. Bagnell, and M. Hebert, “Learning monocular reactive uav control in cluttered natural environments,” in _2013 IEEE international conference on robotics and automation_. IEEE, 2013, pp. 1765–1772.
  * [13] Y. Pan, C.-A. Cheng, K. Saigol, K. Lee, X. Yan, E. A. Theodorou, and B. Boots, “Imitation learning for agile autonomous driving,” _The International Journal of Robotics Research_ , vol. 39, no. 2-3, pp. 286–302, 2020.
  * [14] X. B. Peng, M. Andrychowicz, W. Zaremba, and P. Abbeel, “Sim-to-real transfer of robotic control with dynamics randomization,” in _2018 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2018, pp. 3803–3810.
  * [15] M. Bojarski, D. Del Testa, D. Dworakowski, B. Firner, B. Flepp, P. Goyal, L. D. Jackel, M. Monfort, U. Muller, J. Zhang _et al._ , “End to end learning for self-driving cars,” _arXiv preprint arXiv:1604.07316_ , 2016.
  * [16] A. Giusti, J. Guzzi, D. C. Cireşan, F.-L. He, J. P. Rodríguez, F. Fontana, M. Faessler, C. Forster, J. Schmidhuber, G. Di Caro _et al._ , “A machine learning approach to visual perception of forest trails for mobile robots,” _IEEE Robotics and Automation Letters_ , vol. 1, no. 2, pp. 661–667, 2015.
  * [17] Y. Song, S. Naji, E. Kaufmann, A. Loquercio, and D. Scaramuzza, “Flightmare: A flexible quadrotor simulator,” _arXiv preprint arXiv:2009.00563_ , 2020.
  * [18] W. Guerra, E. Tal, V. Murali, G. Ryou, and S. Karaman, “Flightgoggles: A modular framework for photorealistic camera, exteroceptive sensor, and dynamics simulation,” _arXiv preprint arXiv:1905.11377_ , 2019.
  * [19] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: an open-source library for real-time metric-semantic localization and mapping,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 1689–1696.
  * [20] C. Dawson, B. Lowenkamp, D. Goff, and C. Fan, “Learning safe, generalizable perception-based hybrid control with certificates,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 1904–1911, 2022.
  * [21] Y. Pan, C.-A. Cheng, K. Saigol, K. Lee, X. Yan, E. Theodorou, and B. Boots, “Agile autonomous driving using end-to-end deep imitation learning,” _arXiv preprint arXiv:1709.07174_ , 2017.
  * [22] K. Lee, B. Vlahov, J. Gibson, J. M. Rehg, and E. A. Theodorou, “Approximate inverse reinforcement learning from vision-based imitation learning,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 10 793–10 799.
  * [23] D. Hendrycks, N. Mu, E. D. Cubuk, B. Zoph, J. Gilmer, and B. Lakshminarayanan, “Augmix: A simple data processing method to improve robustness and uncertainty,” _arXiv preprint arXiv:1912.02781_ , 2019.
  * [24] D. Antotsiou, C. Ciliberto, and T.-K. Kim, “Adversarial imitation learning with trajectorial augmentation and correction,” _arXiv preprint arXiv:2103.13887_ , 2021.
  * [25] A. Amini, I. Gilitschenski, J. Phillips, J. Moseyko, R. Banerjee, S. Karaman, and D. Rus, “Learning robust control policies for end-to-end autonomous driving from data-driven simulation,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 1143–1150, 2020.
  * [26] F. Borrelli, A. Bemporad, and M. Morari, _Predictive control for linear and hybrid systems_. Cambridge University Press, 2017.
  * [27] P. O. Scokaert and D. Q. Mayne, “Min-max feedback model predictive control for constrained linear systems,” _IEEE Transactions on Automatic control_ , vol. 43, no. 8, pp. 1136–1142, 1998.
  * [28] A. Bemporad, F. Borrelli, and M. Morari, “Min-max control of constrained uncertain discrete-time linear systems,” _IEEE Transactions on automatic control_ , vol. 48, no. 9, pp. 1600–1606, 2003.
  * [29] D. Q. Mayne, M. M. Seron, and S. Raković, “Robust model predictive control of constrained linear systems with bounded disturbances,” _Automatica_ , vol. 41, no. 2, pp. 219–224, 2005.
  * [30] B. T. Lopez, “Adaptive robust model predictive control for nonlinear systems,” Ph.D. dissertation, Massachusetts Institute of Technology, 2019.
  * [31] D. Q. Mayne, S. V. Raković, R. Findeisen, and F. Allgöwer, “Robust output feedback model predictive control of constrained linear systems,” _Automatica_ , vol. 42, no. 7, pp. 1217–1222, 2006.
  * [32] M. Kögel and R. Findeisen, “Robust output feedback mpc for uncertain linear systems with reduced conservatism,” _IFAC-PapersOnLine_ , vol. 50, no. 1, pp. 10 685–10 690, 2017.
  * [33] J. Lorenzetti and M. Pavone, “A simple and efficient tube-based robust output feedback model predictive control scheme,” in _2020 European Control Conference (ECC)_. IEEE, 2020, pp. 1775–1782.
  * [34] M. Laskey, J. Lee, R. Fox, A. Dragan, and K. Goldberg, “Dart: Noise injection for robust imitation learning,” in _Conference on robot learning_. PMLR, 2017, pp. 143–156.
  * [35] T. D. Barfoot, _State estimation for robotics_. Cambridge University Press, 2017.
  * [36] M. Kamel, M. Burri, and R. Siegwart, “Linear vs nonlinear mpc for trajectory tracking applied to rotary wing micro aerial vehicles,” _IFAC-PapersOnLine_ , vol. 50, no. 1, pp. 3463–3469, 2017.
  * [37] “Qualcomm Flight Pro,” Feb 2022, [Online; accessed 21. Feb. 2022]. [Online]. Available: https://developer.qualcomm.com/hardware/qualcomm-flight-pro
  * [38] C. Griwodz, S. Gasparini, L. Calvet, P. Gurdjos, F. Castan, B. Maujean, G. De Lillo, and Y. Lanthony, “Alicevision meshroom: An open-source 3d reconstruction pipeline,” in _Proceedings of the 12th ACM Multimedia Systems Conference_ , 2021, pp. 241–247.
  * [39] A. Agrawal, R. Verschueren, S. Diamond, and S. Boyd, “A rewriting system for convex optimization problems,” _Journal of Control and Decision_ , vol. 5, no. 1, pp. 42–60, 2018.

OC

    Optimal Control
LQR

    Linear Quadratic Regulator
MAV

    Micro Aerial Vehicle
GPS

    Guided Policy Search
UAV

    Unmanned Aerial Vehicle
MPC

    model predictive control
RTMPC

    robust tube model predictive controller
DNN

    deep neural network
BC

    Behavior Cloning
DR

    Domain Randomization
SA

    Sampling Augmentation
IL

    Imitation learning
DAgger

    Dataset-Aggregation
MDP

    Markov Decision Process
VIO

    Visual-Inertial Odometry
VSA

    Visuomotor Sampling Augmentation
  *[IL]: Imitation learning
  *[RTMPC]: robust tube model predictive controller
  *[DR]: Domain Randomization
  *[SA]: Sampling Augmentation
  *[VSA]: Visuomotor Sampling Augmentation