of 6
Moving Obstacle Avoidance: a Data-Driven
Risk-Aware Approach
Skylar X. Wei

, Anushri Dixit

, Shashank Tomar, and Joel W. Burdick,
Member, IEEE
Abstract
—
This paper proposes a new structured method
for a moving agent to predict the paths of dynamically
moving obstacles and avoid them using a risk-aware model
predictive control (MPC) scheme. Given noisy measure-
ments of the a priori unknown obstacle trajectory, a boot-
strapping technique predicts a set of obstacle trajectories.
The bootstrapped predictions are incorporated in the MPC
optimization using a risk-aware methodology so as to pro-
vide probabilistic guarantees on obstacle avoidance. We
validate our methods using simulations of a multi-rotor
drone that avoids various moving obstacles.
Index Terms
—
Predictive control for linear systems,
Stochastic optimal control, Uncertain systems, Robotics.
I. INTRODUCTION
E
MERGING applications of robots in urban, cluttered, and
potentially hostile environments have increased the im-
portance of online path planning with obstacle behavior clas-
sification and avoidance [1]. Traditionally, robot-obstacle in-
teraction is formulated as the problem of planning a collision-
free path from a starting position to a goal [2]. In environments
with an arbitrary number of moving obstacles and agents with
bounded velocity, this problem is known to be NP-hard [3].
One way to handle dynamic obstacles is to limit their modeled
motions. In [4], the authors assumed a priori knowledge of the
obstacle dynamics or motion patterns. Or, one can plan the
agent’s path off-line using a Probabilistic Roadmap (PRM) in
a field of static obstacles and then replan when dynamical be-
haviors are observed [5]. However, without prior knowledge of
an obstacle’s behavior, a worst-case analysis of unsafe set can
lead to conservative behavior. Potential fields (PFs) are actively
used for dynamic obstacle avoidance: e.g., Lam et al. [6] apply
artificial PFs with stochastic reachable sets in Human-Centered
environments. Slow moving and simple (linear or double
integrator-like) dynamics are assumed. Switching-based plan-
ning methods detect and classify dynamic obstacle behavior
against a set of trajectories, such as constant speed, linear, and
projectile-like motion [7], [8]. Classification-based methods
require distinguishable obstacle behaviors and prior knowledge
about the dynamic environment to generate set trajectories.
This paper presents a new framework for discovering the
dynamics of a priori unknown moving obstacles, forecasting
their trajectories, and providing risk-aware optimal avoidance
strategies. It replaces the need for obstacle trajectory/model
classification while allowing online computation. Extracting
a dynamics model from data is challenging [9], especially

Both authors contributed equally.
The authors are with the Division of Engineering
&
Applied Science,
California Institute of Technology, MC 104-44, Pasadena, CA 91125,
({swei, adixit, stomar, jburdick}@caltech.edu).
when the available data is limited, noisy, and partial. To
tackle partial measurements, we leverage Takens’ embedding
theorem [10], which uses partial observations to produce
an attractor that is diffeomorphic to the full-state attractor.
We then use Singular Spectrum Analysis (SSA) [11], [12]
to separate noise from the underlying signal and to extract
a predictive model of obstacle behavior. Our use of time
delay embedding is also the basis of Eigensystem Realization
Algorithm (ERA) in linear system identification [13]. Inspired
by [14], we use a classical bootstrap to forecast a set of
obstacle trajectories with statistical quantification. An MPC
planner then incorporates the set of obstacle forecasts as an
affine conservative approximation of a distributionally-robust
chance constraint (DRCC). This constraint is then efficiently
recast in a risk-aware manner, allowing an MPC optimization
based on sequential convex programming [15], [16].
We demonstrate our approach on three scenarios that exhibit
increasingly complicated dynamical behavior. Monte-Carlo
simulations verify the planner’s ability to uphold the user cho-
sen chance constraint. The risk-aware reformulation not only
gives provable probabilistic collision avoidance guarantees, but
also allows an on-line execution of the planner.
Notation:
The set of positive integers, natural numbers, real
numbers, and positive reals are denoted as
Z
+
,
N
,
R
, and
R
+
,
respectively. We denote the sequence of consecutive integers
f
i;

;i
+
k
g
as
Z
i
:
i
+
k
. The finite sequence
f
a
1
;

;a
k
g
of
scalars or vectors
a
is denoted as
f
a
g
k
1
. The expression
I
n

n
denotes
n
by
n
identity matrices and
1
= [1
;
1
;
1]
T
:
II. SSA P
RELIMINARIES
Consider a discrete-time multivariate stochastic process
f
o
m
g
N
1
where
m
denotes the
m
th
observable of the process out
of the total
N
o
available observables, and
N
is the number of
available observations. Suppose that the true stochastic process
model of the observables is
^
o
m
=
o
m
+
m
, where
m
de-
notes a random discrete-time zero-mean measurement noise,
and
o
m
is the noiseless observable that captures the governing
laws, which can be composed of
trends
,
seasons
, and
sta-
tionary
time series. Singular Spectrum Analysis [11] separates
the true signal
o
m
and the noise
m
and extracts a recursive
governing dynamic model of
o
m
that can generate a short term
accurate forecast.
Fig.
1
describes this method
.
1) Time Delay Embedding
:
Takens’ method of delays [10] can
reconstruct qualitative features of the full-state phase-space
from delayed partial observations. The
m
th
-state observables
^
o
m
are delay embedded into a trajectory (Hankel) matrix
H
m
[
L

N
]
,
Fig.
1
gives an example of the Hankel matrix for
state
x
. Parameter
L
is the time delay length, and
N
is the
This article has been accepted for publication in IEEE Control Systems Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LCSYS.2022.3181191
This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
Fig. 1
: A description of bootstrap-SSA-forecast architecture in forecasting the trajectory of a Frisbee where the stochastic observables (corrupted by zero-mean,
noise) consist of
f
^
o
g
N
1
= [
f
^
x
g
N
1
;
f
^
y
g
N
1
;
f
^
z
g
N
1
]
, the Frisbee’s center positions with respect to an inertial frame. The SSA analysis and bootstrap forecast
is applied to every observable state. Despite its 12-state governing dynamics [17] and with only center position measurements of the Frisbee, we show an
example
N
strap
forecasts of the Frisbee trajectory for future time steps
f
1
;
2
;

; N
h
g
using our proposed framework.
time series length. Repeating patterns in the Hankel matrix
represent underlying trends and oscillations, which can be
extracted from its covariance matrix:
X
m
=
H
m
[
L;N
]
(
H
m
[
L;N
]
)
T
.
2) Eigen Decomposition
:
To recover the true signal
o
m
, we
seek the best, low-rank matrix approximation of this sig-
nal by thresholding the eigenvalues of
X
m
[18]. The sym-
metric covariance matrix
X
m
has a spectral decomposition
U

U
T
, where

is a diagonal matrix with real eigenvalues

1


2
 

L
. The matrix of left eigenvectors
U
=
[

1
;

;

L
]
is orthogonal. The truncated right eigenvectors
V
= [

1
;

;

L
]
T
2
R
L

N
of
X
m
can be found as
V
=
U

.
Suppose

is the optimal threshold and

n




n
+1
, which
partitions the Hankel matrix
H
m
[
L;N
]
as:
H
m
[
L;N
]
=
n
∑
p
=1
√

p

p

T
p
|
{z
}
,
H
o
[
L;N
]
+
L
∑
p
=
n
+1
√

p

p

T
p
|
{z
}
,
H
[
L;N
]
:
(1)
3) Hankelization
:
Matrix
H
o
[
L;N
]
in (
1
) should maintain a Han-
kel structure: minor variations in its
k
th
secondary diagonals
result from insufficient noise removal.
1
A
Hankelization step
performs secondary diagonal averaging in order to find the
matrix
H
O
that is closest to
H
o
[
L;K
]
with respect to the
Frobenius norm among all
L

N
Hankel matrices [11]. The
operator
H
acting on
L

N
matrix
H
y
[
L;N
]
entry wise is
defined as: for the
(
i;j
)
th
element of matrix
H
o
[
L;N
]
and
i
+
j
=
s
, define a set
D
s
,
f
(
l;n
) :
l
+
n
=
s;l
2
Z
1:
L
;n
2
Z
1:
N
g
, is
mapped to
(
i;j
)
th
element of the Hankelized
H
H
o
[
L;N
]
via the
expression in Fig.
1
(for the case of
o
m
=
x
), where
j
D
s
j
de-
notes the number of elements in set
D
s
.
4) Forecast with Linear Recurrence Formula
:
Definition 1.
A time series
Y
N
=
f
y
g
N
1
admits an L-
decomposition of order not larger than d, denoted by
ord
L
(
Y
N
)

d
, if there exist two systems of functions
ρ
k
:
Z
0:
L
1
!
R
;#
k
:
Z
0:
L
1
!
R
;
such that
y
i
+
j
=
∑
d
k
=1
#
k
(
i
)
ρ
k
(
j
)
;
f
i;j
g2
Z
0:
L
1

Z
0:
L
1
8
k
2
Z
1:
d
.
1
The
k
th
secondary diagonals of a matrix
M
are also the
k
th
diagonals of
M
flipped horizontally with respect to its middle column.
If ord
L
(
Y
N
) =
d
, then the series
Y
N
admits a
L-decomposition
of the order d
and both systems of functions
(
ρ
1
;

;ρ
d
)
and
(
#
1
;

;#
d
)
are linearly independent [19].
Definition 2.
A time series
f
y
g
N
1
is governed by a linear
recurrent relations/formula (LRF), if there exist coefficients
f
φ
g
d
1
and
φ
d
̸
= 0
such that
y
i
+
d
=
d
∑
k
=1
φ
k
y
i
+
d
k
;
8
i
2
Z
0:
N
d
;d < N :
(2)
Real-valued time series governed by LRFs consists of sums
of products of polynomials, exponentials and sinusoids [11].
Theorem 1.
[11] Let

1:
L
1
i
be the vector of the first
L
1
components of a left eigenvector

i
of
H
m
[
L;N
]
, and let

i
be the
L
th
component of eigenvector

i
. Let
v
2
,
∑
d
i
=1

2
i
.
Under Assumptions
2
and
3
(see below), the LRF coefficients
φ
i
where
i
2
[1
;L
1]
can be computed as:
[
φ
L
1
φ
L
2

φ
1
]
T
=
1
1
v
2
d
∑
i
=1

i

1:
L
1
i
;
(3)
and
y
evolves as the LRF:
y
N
+1
=
∑
L
1
j
=1
φ
j
y
N
j
.
III. P
ROBLEM
S
TATEMENT
Consider the linear, discrete-time dynamical agent model:
x
i
+1
=
A
x
i
+
B
u
i
;
y
i
+1
=
G
x
i
+1
;
(4)
where
x
i
2
R
n
x
,
u
i
2
R
n
u
, and
y
i
2
R
n
y
, for all
i
2
N
, correspond to the system states, controls, and outputs
at time index
i
respectively. The state transition, actuation,
and measurement matrices are
A
2
R
n
x

n
x
,
B
2
R
n
x

n
u
,
and
G
2
R
n
y

n
x
respectively. Constant matrix
C
2
R
3

n
x
maps the system’s states (
4
) to the system’s
x;y;z
positions
with respect to inertial frame
E
. We model the
k
th
obstacle,
k
2
Z
1:
N
obs
, as a sphere. The obstacle occupies the point set
O
k
(
c
k
;r
k
) =
f
x
2
R
3
:
∥
c
k
x
∥
2

r
k
g
, where
c
k
2
R
3
and
r
k
2
R
+
are the
k
th
obstacle’s center and radius.
We consider the case where the agent (
4
) is tasked with
following a reference output trajectory
y
ref
which need not
consider obstacle information. While following this path, the
This article has been accepted for publication in IEEE Control Systems Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LCSYS.2022.3181191
This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/
agent may encounter
N
obs
spherical, stationary or moving
obstacles. The obstacle-free region is the open set:
S
,
{
R
3
n[
N
obs
k
=1
O
k
}
:
(5)
Assumption 1.
Obstacles can be detected and localized at the
same rate (
f
+
Hz) as the planner update. Only measurements
of an obstacle’s geometric center with respect to frame E are
assumed, and they are corrupted by a zero-mean noise. We
can estimate the radius,
r
k
, of the
k
th
obstacle as
^
r
k
, and the
estimate satisfies
^
r
k

r
k
.
2
Assumption 2.
All obstacle measurements, admit an L-
decomposition of order
d
, are governed by LRFs
(
2
)
whose
LRF coefficients can be uniquely defined.
Assumption 3.
We assume that the obstacles’ velocities
are bounded by
v
max
, and the initial distances between all
obstacles and the agent are significantly greater than
dv
max
f
+
.
Problem 1.
[Prediction]
Consider a multivariate stochastic
process where observables
f
x
g
N
1
,
f
y
g
N
1
, and
f
z
g
N
1
denote the
spherical obstacle’s true center location in reference frame,
E. The measurements are corrupted by independent, zero-
mean noises
f
1
g
N
1
,
f
2
g
N
1
, and
f
3
g
N
1
(see Fig.
1
). Under
Assumptions
1
-
3
, we seek to predict the obstacle position at
times
N
+ 1
to
N
+
N
h
using these measurements.
Due to limited and noisy partial data and the lack of explicit
dynamics models, we estimate a Bootstrap distribution of the
obstacle predictions, denoted by the random set
O
pred
, from
time index
N
+ 1
to
N
+
N
h
and calculate its first and
second moments. We account for errors in the forecastS due
to poor signal and noise separation and bandwidth limits (due
to limited training data and incorrect choices of embedding
length
L
) by solving a DRCC MPC problem.
Problem 2.
[Planning]
Consider the system
(
4
)
and free-
space
(
5
)
. Given a discrete-time reference trajectory
y
ref
i
8
i
2
Z
1:
N
h
where
N
h
2
Z
+
is the length of the horizon, convex
state constraints
D
x

R
n
x
, convex input constraints
D
u

R
n
u
, and a convex stage cost
L
i
:
R
n
x

R
n
u
!
R

0
, a total
of
N
obs
spherical obstacles each approximated by a set
O
pred
k
,
and risk tolerance
ε
2
(0
;
1]
, we seek to compute a receding
horizon controller
f
u

g
N
h
1
that avoids the unsafe set
O
pred
,
∪
N
obs
k
=1
O
pred
k
via the following non-convex optimization:
f
u

g
N
h
1
=
min
f
u
k
g
N
h
1
2
R
n
u
N
h
∑
i
=1
L
i
(
y
ref
i
y
i
;
u
i
)
(6a)
s.t.
x
i
+1
=
A
x
i
+
B
u
i
y
i
+1
=
G
x
i
+1
(6b)
x
i
2D
x
;
u
i
2D
u
;
x
1
=
x
init
(6c)
P
(
x
i
2O
pred
)

ε;
8
i
2
Z
1:
N
h
(6d)
IV. B
OOTSTRAP
F
ORECASTING
Despite empirical successes in reconstructing and forecasting
[12], the theoretical accuracy of SSA is strenuous to obtain,
see [20]. Inspired by [14], we use bootstrapping to improve
model discovery and to produce probabilistic forecasts.
2
Note Assumption
1
does not
imply full state measurement.
Our real-time bootstrap forecast, Algorithm
1
, assumes time
series measurements corrupted by noise. The user-defined
parameters
N
train
and
N
step
represent the number of initial
training samples, and the number of newly accumulated sam-
ples during an initial bootstrap. Further, one must choose
parameters

t
and
N

, where threshold

t
is used to sep-
arate signal from noise, and
N

is the number of steps
of progressive relaxation of threshold

t
.
3
In the desired
signal/noise separation (
1
), the unknown theoretical optimal
threshold

must be estimated. Let
Y

1
:

d
N
be the Hanke-
lization reconstructed
^
y
with the eigenvalues
f

g
d
1
and their
corresponding right and left eigenvectors. Note, if
d > n
where

n




n
+1
, then the norm values
∥
Y

1
:

d
+
t
N
Y

1
:

d
+
t
+1
N
∥
2
 ∥
Y

1
:

d
+
t
+1
N
Y

1
:

d
+
t
+2
N
∥
2
since they are
comprised of the residual measurement noise. We threshold
the difference between two consecutive reconstructions with

t
=N
, i.e. finding the smallest
t
2
Z
+
s.t.:
∥
Y

1
:

t
N
Y

1
:

t
+1
N
∥
2
∥
Y

1
:

t
+1
N
Y

1
:

t
+2
N
∥
2


t
N
(7)
Since the selection of the threshold

t
is crucial, we add an
additional parameter
N

to ensure no principle components
are lost in
Y

1
:

d
N
because of bad choice of

t
, i.e. to avoid
d < n
. We also include the next
N

largest eigenvalues
after the first
t
eigenvalues in the bootstrapping process. Most
importantly, the number of bootstraps,
N
strap
, needs to be
determined
a priori
, considering the computation capacity,
number of obstacles, and the expected noise level
4
.
Algorithm 1
Bootstrap Forecast Algorithms (Per Obstacle)
Data:
Obstacle center position measurements
f
^
x
g
N
1
;
f
^
y
g
N
1
;
f
^
z
g
N
1
,
User defined constants:
N
train
,
N
step
,

t
,
N

,
N
strap
Result:
Forecast:
f
j
x
g
N
+
N
h
N
+1
;
f
j
y
g
N
+
N
h
N
+1
;
f
j
z
g
N
+
N
h
N
+1
;
8
j
2
Z
1:
N
straps
Use
f
^
x
N
+1
;
^
y
N
+1
,
^
z
N
+1
g
to update Hankel matrix
while
istrap

N
strap
do
while
N
+ 1

N
train
do
for
observables
= 1 :
N
o
do
while
(
7
)
holds
do
t
++
end
obtain (
f

istrap
g
t
1
,
f

istrap
g
t
1
,
φ
istrap
) for each states, istrap
++
for
tt
=
t
+ 1 :
t
+
N

do
obtain (
f

istrap
g
tt
1
,
f

istrap
g
tt
1
,
φ
istrap
) for each states,
istrap
++
end
end
N
train
=
N
train
+
N
step
end
Back-up Strategy
end
Apply the tuples (
f
j

istrap
g
t
j
1
,
f
j

istrap
g
t
j
1
,
j
φ
istrap
)
8
j
2
Z
1:
Nstraps
for
x; y; z
to the updated Hankel, where
t
j
denotes number of eigenvalues post
truncation for the
j
th
bootstrap. Perform a
N
h
step forecast using
j
φ
istrap
.
V. B
OOTSTRAP
P
LANNING
This section introduces an MPC-based path planner to solve
Problem
2
. First, we reformulate the obstacle avoidance
3
The parameters

t
and
N

are dictated by measurement noise levels,
which can be characterized off-line in a controlled experimental setting.
4
The effectiveness of Algorithm
1
depends highly on the time delay length
L
, the number of training measurements
N
train
, the number of bootstraps
N
strap
, and the MPC horizon length,
N
h
. We recommend that
N
train
be at
least
10
N
h
and that
L
=
N
train
4
.
N
strap
and
N
step
should be as large as
allowed by the computing platform and benchmarking them offline.
This article has been accepted for publication in IEEE Control Systems Letters. This is the author's version which has not been fully edited and
content may change prior to final publication. Citation information: DOI 10.1109/LCSYS.2022.3181191
This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/