Moving Obstacle Avoidance: a Data-Driven Risk-Aware Approach
Skylar X. Wei
∗
, Anushri Dixit
∗
, Shashank Tomar, and Joel W. Burdick
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 measurements of the a
priori unknown obstacle trajectory, a bootstrapping technique
predicts a set of obstacle trajectories. The bootstrapped predic-
tions are incorporated in the MPC optimization using a risk-
aware methodology so as to provide probabilistic guarantees on
obstacle avoidance. We validate our methods using simulations
of a 3-dimensional multi-rotor drone that avoids various moving
obstacles, such as a thrown ball and a frisbee with air drag.
I. INTRODUCTION
Emerging applications of robots in urban, cluttered, and po-
tentially hostile environments have increased the importance
of online path planning with obstacle behavior classification,
and avoidance [1]. Traditionally, the interaction of a robot
with an obstacle is formulated as the problem of planning a
collision-free path to navigate from a starting position to a
goal [2]. In dynamic 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 behaviors are observed [5]. However,
without prior knowledge of the obstacle behavior, a worst-
case analyses of unsafe obstacle locations can lead to overly
conservative behaviors. Potential fields (PFs) have been
actively used for dynamic obstacle avoidance: e.g., recent
works [6] apply artificial PFs with stochastic reachable sets
in Human-Centered environments. Despite its computational
efficiency and scalability with the number of obstacles,
slow moving and simple (linear or double integrator-like)
dynamics are assumed. Switching-based planning 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 a generate set of 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
∗
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).
classification, while allowing an online implementation. Ex-
tracting a dynamics model from data is challenging [9] and
the difficulty increases when the available data is limited,
noisy, and partial. To tackle the partial measurement is-
sue, we leverage Takens embedding theorem [10], which
enables 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 recurrence model to
predict the obstacle behavior. Note that our use of time
delay embedding is also the basis of Eigensystem Realization
Algorithm (ERA) in linear system identification [13] and
has been connected with Koopman operators [9]. Inspired by
[14], we employ a classical bootstrap method to forecast a set
of moving obstacle trajectories with statistical quantification.
We propose an MPC planner that incorporates the set of
obstacle forecasts as an affine conservative approximation of
a distributionally-robust chance constraint. This constraint is
then efficiently reformulated in a risk-aware way, allowing
the MPC optimization to solved using a sequential convex
programming approach [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
chosen chance constraint. The risk-aware reformulation not
only gives provable probabilistic collision avoidance guaran-
tees, but also allows on-line execution of the planner.
Notation:
The set of positive integers, natural number, real
numbers, and positive real numbers are denoted as
Z
+
,
N
,
R
,
and
R
+
, respectively. We denote the sequence of consecutive
integers
{
i,i
+ 1
,
···
,i
+
k
}
as
Z
i
:
i
+
k
. The finite sequence
{
a
1
,
···
,a
k
}
of a scalar or vector variable
a
is denoted as
{
a
}
k
1
. The expression
I
n
×
n
is used to denote
n
by
n
identity
matrices and
1
= [1
,
1
,
1]
T
.
II. SSA P
RELIMINARIES
Consider a discrete-time multivariate stochastic process
{
o
m
}
N
1
where
m
denotes the
m
th
observable measurement
of the process, and
N
is the total number of available
observations, i.e.,
o
m
i
denotes the
m
th
observation variable
at process sampling index
i
. Suppose the true model of the
stochastic process in terms of the observables is
[
ˆ
o
m
1
···
ˆ
o
m
N
]
︸
︷︷
︸
ˆ
o
m
=
[
o
m
1
···
o
m
N
]
︸
︷︷
︸
o
m
+
[
γ
m
1
···
γ
m
N
]
︸
︷︷
︸
γ
m
(1)
where
γ
m
denotes a random discrete-time zero-mean mea-
surement noise, and
o
m
is the noiseless observable that
captures the governing laws, which can be composed of
arXiv:2203.14913v1 [cs.RO] 25 Mar 2022
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
{
ˆ
o
}
N
1
= [
{
ˆ
x
}
N
1
,
{
ˆ
y
}
N
1
,
{
ˆ
z
}
N
1
]
, the Frisbee’s center positions with respect to an inertial frame. The SSA analysis and bootstrap
forecast is applied to every observable states as indicated. 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
{
1
,
2
,
···
,N
h
}
using our proposed framework.
trends
,
seasons
, and
stationary
time series. Singular Spec-
trum 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.
1) Time Delay Embedding:
Taken’s method of delays [10]
can be used to reconstruct qualitative features of the full-
state, phase space from delayed partial observations. The
m
th
state raw observables
ˆ
o
m
are delay embedded into the
following trajectory (Hankel) matrix:
H
m
[
L,N
]
=
ˆ
o
m
1
ˆ
o
m
2
...
ˆ
o
m
N
−
L
+1
ˆ
o
m
2
ˆ
o
m
3
...
ˆ
o
m
N
−
L
+2
.
.
.
.
.
.
.
.
.
.
.
.
ˆ
o
m
L
ˆ
o
m
L
+1
...
ˆ
o
m
N
(2)
where
L
represents time delay length and
N
is length of
the time series. The repeating patterns in the Hankel matrix
represent underlying trends and oscillations, and 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
aim to find the best, low-rank matrix approximation of the
true signals by thresholding the eigenvalues of
X
m
, similar
to [18]. More specifically, the symmetric 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
∈
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,T
]
as:
H
m
[
L,T
]
=
n
∑
p
=1
√
λ
p
μ
p
ν
T
p
︸
︷︷
︸
,
H
o
[
L,K
]
+
L
∑
p
=
n
+1
√
λ
p
μ
p
ν
T
p
︸
︷︷
︸
,
H
γ
[
L,K
]
.
(3)
3) Hankelization:
Matrix
H
o
[
L,K
]
(3) should maintain a Han-
kel structure, and minor variations in its
k
th
secondary
diagonals result from insufficient noise removal.
1
Therefore,
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.
a Hankelization step is introduced to perform secondary
diagonal averaging, finding the matrix
H
O
that is closest
to
H
o
[
L,K
]
with respect to the Frobenius norm among all
Hankel matrices of size
L
×
N
[11]. The operator
H
acting
on an arbitrary
L
×
N
matrix
H
y
[
L,N
]
entry wise is defined as
follows: for the
(
i,j
)
th
element of matrix
H
o
[
L,N
]
and
i
+
j
=
s
,
define a set
D
s
,
{
(
l,n
) :
l
+
n
=
s,l
∈
Z
1:
L
,n
∈
Z
1:
N
}
, 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
|
D
s
|
de-
notes the number of elements in the set
D
s
.
4) Forecast with Linear Recurrence Formula:
Definition 1.
A time series
Y
N
=
{
y
}
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
θ
(
i
)
%
k
(
j
)
i,j
∈
Z
0:
L
−
1
×
Z
0:
L
−
1
for all
k
∈
Z
1:
d
.
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
{
y
}
N
1
is governed by a linear
recurrent relations/formula (LRF), if there exist coefficients
{
φ
}
m
1
and
φ
m
6
= 0
such that
y
i
+
d
=
d
∑
k
=1
φ
k
y
i
+
d
−
k
,
∀
i
∈
Z
0:
N
−
d
,d < N .
(4)
Real-valued time series governed by LRFs consists of sums
of products of polynomials (trends), exponentials (stationary,
linear time invariant) and sinusoids (seasons) [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
∈
[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
(5)
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
(6)
where
x
i
∈
R
n
x
,
u
i
∈
R
n
u
, and
y
i
∈
R
n
y
for all
i
∈
N
correspond to the system state, controls, and output at
time index
i
respectively. The state transition, actuation, and
measurement matrices are
A
∈
R
n
x
×
n
x
,
B
∈
R
n
x
×
n
u
, and
G
∈
R
n
y
×
n
x
respectively. Let
C
∈
R
3
×
n
x
be a constant
matrix that maps the system’s states (6) to the system’s
Cartesian
x,y,z
positions with respect to an inertial frame
E
. We model the
k
th
obstacle,
k
∈
Z
1:
N
obs
, as a sphere.
The set of Cartesian points occupied by the obstacle is
O
k
(
c
k
,r
k
) =
{
x
∈
R
3
:
‖
c
k
−
x
‖
2
≤
r
k
}
, where
c
k
∈
R
3
and
r
k
∈
R
+
are the center and radius of the
k
th
obstacle.
This paper considers the case where the agent (6) is tasked
with following a specified reference output trajectory
y
ref
,
whose geometry need not incorporate any obstacle informa-
tion. While following this path, the agent may encounter
N
obs
spherical stationary or moving obstacles. The obstacle-
free region is given by the open set:
S
,
{
R
3
\∪
N
obs
k
=1
O
k
}
.
(7)
Assumption 1.
Obstacles can be detected and localized
at the same rate (
f
+
Hz) of 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
(4)
whose
LRF coefficients can be uniquely defined.
Assumption 3.
We assume that the obstacles’ velocities are
bounded by
v
max
, and the initial displacements between all
obstacles and the agent are significantly greater than
dv
max
f
+
.
Problem 1.
[Prediction]
Consider a multivariate stochastic
process where observables
{
x
}
N
1
,
{
y
}
N
1
, and
{
z
}
N
1
corre-
spond to the spherical obstacle’s true center location with
respect to a common reference frame, E. The measurements
are corrupted by independent, zero-mean noises
{
γ
1
}
N
1
,
{
γ
2
}
N
1
, and
{
γ
3
}
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 measurements where
N
h
∈
Z
+
.
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 forecast locations
due to poor signal and noise separation and bandwidth
limits (due to limited training data and incorrect choices
2
It is important to note that Assumption 1
does not
imply full state
measurement. See Fig. 1 for an example of Frisbee.
of embedding length
L
) by solving a distributionally robust
chance constrained model predictive planning problem.
Problem 2.
[Planning]
Consider the system
(6)
and free-
space
(7)
. Given a discrete-time reference trajectory
y
ref
i
∀
i
∈
Z
1:
N
h
where
N
h
∈
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 function
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
∈
(0
,
1]
, we seek to
compute a receding horizon controller
{
u
∗
}
N
h
1
that avoids
the unsafe set
O
pred
,
⋃
N
obs
k
=1
O
pred
k
via the following non-
convex optimization problem:
{
u
∗
}
N
h
1
=
min
{
u
k
}
N
h
1
∈
R
n
u
N
h
∑
i
=1
L
i
(
y
ref
i
−
y
i
,
u
i
)
(8a)
s.t.
x
i
+1
=
A
x
i
+
B
u
i
y
i
+1
=
G
x
i
+1
(8b)
x
i
∈D
x
,
u
i
∈D
u
,
x
1
=
x
init
(8c)
P
(
x
i
∈O
pred
)
≤
,
∀
i
∈
Z
1:
N
h
(8d)
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.
Algorithm 1
Bootstrap Forecast Algorithms (Per Obstacle)
Data:
Obstacle center position measurements
{
ˆ
x
}
N
1
,
{
ˆ
y
}
N
1
,
{
ˆ
z
}
N
1
,
User defined constants:
N
train
,
N
step
,
δ
t
,
N
σ
,
N
strap
Result:
Forecast:
{
j
x
}
N
+
N
h
N
+1
,
{
j
y
}
N
+
N
h
N
+1
,
{
j
z
}
N
+
N
h
N
+1
,
∀
j
∈
Z
1:
N
straps
Use
{
ˆ
x
N
+1
,
ˆ
y
N
+1
,
ˆ
z
N
+1
}
to update Hankel matrix
while
istrap
≤
N
strap
do
while
N
+ 1
≥
N
train
do
for
states
=
x,y,z
do
while
‖
Y
λ
1
:
λ
t
N
+1
−
Y
λ
1
:
λ
t
+1
N
+1
‖
2
−‖
Y
λ
1
:
λ
t
+1
N
+1
−
Y
λ
1
:
λ
t
+2
N
+1
‖
2
≥
δ
t
N
+1
do
t
=
t
+ 1
end
obtain the tuple for each states:(
{
λ
istrap
}
t
1
,
{
μ
istrap
}
t
1
,
φ
istrap
),
istrap
=
istrap
+ 1
for
tt
=
t
+ 1 :
t
+
N
σ
do
obtain the tuple for states: (
{
λ
istrap
}
tt
1
,
{
μ
istrap
}
tt
1
,
φ
istrap
),
istrap
=
istrap
+ 1
end
end
N
train
=
N
train
+
N
step
end
Back-up Strategy
end
Apply the tuples (
{
j
λ
istrap
}
t
j
1
,
{
j
μ
istrap
}
t
j
1
,
j
φ
istrap
)
∀
j
∈
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
.
Our real-time bootstrap forecast, Algorithm 1, assumes time
series measurements of the form (1). The user-defined param-
eters
N
train
and
N
step
represent the allowed number of initial
training samples, and the number of newly accumulated
samples during an initial bootstrap. Further, one must choose
parameters
δ
t
and
N
σ
, where
δ
t
is the threshold used to
separate signal from noise, and
N
σ
is the number of steps
of progressive relaxation of threshold
δ
t
.
3
Recall the desired
3
The parameters
δ
t
and
N
σ
are dictated by measurement noise levels,
which can be characterized off-line in a controlled experimental setting.
signal/noise separation (3), the theoretical optimal threshold
λ
is unknown and must be estimated. Let
Y
λ
1
:
λ
d
N
be the
Hankelization reconstructed
ˆ
y
with the eigenvalues
{
λ
}
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
∈
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
(9)
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
. To be conservative, we 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.
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.
V. B
OOTSTRAP
P
LANNING
This section introduces an MPC-based path planner to solve
Problem 2. First, we revisit the obstacle avoidance constraint
(8d) and its properties given the mean and variance of the
bootstrap predictions. Next, we use this obstacle avoidance
constraint in the MPC optimization, and provide probabilistic
guarantees of constraint satisfaction. Algorithm 1 produces
N
strap
copies of
N
h
length predictions of the
k
th
obstacle’s
location. We denote the
j
th
copy of the bootstrap prediction
as
{
ˆ
y
j
k
}
N
h
1
=
{
ˆ
y
j
1
,k
,
ˆ
y
j
2
,k
,
···
,
ˆ
y
j
N
h
,k
}
. The collision avoidance
set constraint (8d) can be reformulated based on the obstacle
shape and center as
‖
C
x
i
−
ˆ
y
j
i,k
‖
2
≥
ˆ
r
k
+
r
p
,
r
k
, for each
i
∈
Z
1:
N
h
and
k
∈
Z
1:
N
obs
and
r
p
is the safety radius of the
agent (6). This constraint can be equivalently expressed as
the following concave (in the state
x
i
) constraint,
(
C
x
i
−
ˆ
y
j
i,k
)
T
(
C
x
i
−
ˆ
y
j
i,k
)
≥
r
k
‖
(
C
x
i
−
ˆ
y
j
i,k
)
‖
2
.
(10)
We approximate (10) as an affine constraint through the use
of Sequential Convex Programming (SCP) [15], [16]
(
C
x
i
−
ˆ
y
j
i,k
)
T
(
C
x
i
−
ˆ
y
j
i,k
)
≥
r
k
‖
(
C
x
i
−
ˆ
y
j
i,k
)
‖
2
(11)
where
x
i
is approximated with the solution from previous
SCP iterations. Note that Eq. (11) over-approximates con-
straint (10) (see [15] for proof).
Lemma 2.
If we have
N
strap
forecasts of the
k
th
obstacle’s
position from time
i
∈
Z
1:
N
h
and the previous SCP trajectory
{
x
}
N
h
1
, then we can define the
j
th
bootstrap lumped collision
avoidance coefficients
α
j
i,k
,
β
j
i,k
and the standard deviation
of the collision avoidance constraint
∆
i,k
as:
α
j
i,k
,
−
C
T
(
C
x
i
−
ˆ
y
j
i,k
)
(12)
β
j
i,k
,
r
k
‖
(
C
x
i
−
ˆ
y
j
i,k
)
‖
2
−
(
C
x
i
)
T
(
C
x
i
−
ˆ
y
j
i,k
)
(13)
∆
i,k
,
√
p
T
i
Σ
α
i,k
p
i
+ 2
p
T
k
Σ
αβ
i,k
+ Σ
βi,k
,
(14)
where,
Σ
α
i,k
,
cov
(
α
j
i,k
,α
j
i,k
)
,
Σ
β
i,k
,
cov
(
β
j
i,k
,β
j
i,k
)
,
and
Σ
αβ
i,k
,
cov
(
α
j
i,k
,α
j
i,k
)
are sample covariance
matrices computed using the bootstrapped coefficients
{
α
i,k
}
N
strap
1
and
{
β
i,k
}
N
strap
1
and
p
i
,
C
x
i
∈
R
3
. Let the
dimension of the null space of
Σ
α
i,k
be
n
i,k
≥
0
.
4
The
standard deviation
∆
i,k
has the following upper bound,
∆
i,k
≤
1
T
|
̃
Σ
1
2
α
i,k
(
p
i
−
h
i,k
)
|
+
√
3
k
i,k
,
ζ
i,k
,
(15)
where
̃
Σ
α
i,k
= Σ
α
i,k
+
I
null
i,k
,
I
null
i,k
=
[
00
0
I
n
i,k
×
n
i,k
]
∈
R
3
×
3
, and
[
h
i,k
k
i,k
]
,
[
−
(
Σ
α
i,k
+
I
null
i,k
)
−
1
Σ
αβ
i,k
Σ
β
i,k
−
Σ
T
αβ
i,k
(
Σ
α
i,k
+
I
null
i,k
)
−
1
Σ
αβ
i,k
]
.
(16)
Proof.
See Appendix.
Note that while all bootstraps can be incorporated as separate
obstacle avoidance constraints, such operations are costly,
as the number of constraints grows linearly with
N
strap
.
Instead, we estimate the ensemble mean and covariance of
the distance from the obstacle. The theorem below uses a
distributionally robust chance constraint to account for
all
bootstrap distributions
that can have this mean and covari-
ance. This approach results in a significantly reduced number
of obstacle avoidance constraints, and this number remains
fixed regardless of the number of bootstrap predictions
N
strap
.
Theorem 3.
(SSA-MPC) Consider Problem 2 under As-
sumptions 1-3 with system dynamics
(6)
and bootstrap SSA
forecasts of all obstacles’ center positions. Given a risk
tolerance
, the solution to the following optimal control
problem is a feasible solution of Problem 2 as
w
−→ ∞
.
The SCP optimization problem at iteration
w
is:
{
u
∗
}
N
h
1
=
min
u
i
∈
R
n
u
s
i,k
∈
R
3
N
h
∑
i
=1
L
i
(
y
ref
i
−
G
x
i
,
u
i
)
(17a)
s.t.
x
i
+1
=
A
x
i
+
B
u
i
(17b)
x
i
∈D
x
,
u
i
∈D
u
,
x
1
=
x
init
(17c)
Λ
i,k
[
x
i
s
i,k
]
T
≤
Γ
i,k
(17d)
‖
x
i
−
x
i
‖≤
χτ
w
∀
i,k
∈
Z
1:
N
h
×
Z
1:
N
obs
(17e)
where
{
̄
x
}
N
h
1
is the solution to the
(
w
−
1)
th
iteration of the
SCP optimization,
Λ
i,k
∈
R
7
×
11
and
Γ
i,k
∈
R
7
encode the
risk-based collision avoidance relationships,
Λ
i,k
=
E
[
α
i,k
]
T
C
1
T
ν
n
̃
Σ
1
/
2
α
i,k
C
−
I
3
×
3
−
̃
Σ
1
/
2
α
i,k
C
−
I
3
×
3
,
Γ
i,k
=
−
E
[
β
i,k
]
−
ν
n
√
3
k
i,k
̃
Σ
1
/
2
α
i,k
h
i,k
−
̃
Σ
1
/
2
α
i,k
h
i,k
.
4
For all our numerical simulation,
Σ
α
i,k
is strictly positive definite.
However, in the case of one or multiples measurable states are noiseless,
Σ
α
i,k
can be ill-conditioned. Alternative to adding
I
null
i,k
which can be
numerically expansive to determine, we recommend applying Algorithm 1
only to states that measurement noises are present and adapt Theorem 3 with
deterministic forecasts for the states without noise and the distributionally
robust chance constraint formulation for the noisy ones.
Fig. 2: Four Monte-Carlo simulations with agent dynamics (20) and a Frisbee obstacle (see Fig.1) are compared. The same obstacle behaviors are
simulated while the agent tracks the same figure ’8’ reference trajectory with four risk levels
=
{
0
.
05
,
0
.
25
,
0
.
5
,
1
}
. The simulation is designed to be
difficult: the vehicle must deviate from its reference trajectory as the obstacle trajectory is designed to intersect the agent’s reference trajectory with noise
obstacle trajectory measurements. All measurement noises are sampled uniformly between
[
−
0
.
125
,
0
.
125]
meters. The bootstrap obstacle forecast uses
the parameters:
L
= 24
,
N
train
= 100
,
N
step
= 5
,
δ
t
= 20
,
N
σ
= 8
,
N
strap
= 40
. SSA-MPC uses the constants
N
h
= 10
,
χ
= 50
and
τ
= 0
.
25
with fixed 4-step SCP iterations. The tuple (
{
λ
j
}
t
j
1
,
{
μ
j
}
t
j
1
,
φ
j
),
∀
j
∈
Z
1:40
in Algorithm 1 is computed with observables measured at 20 Hz. The four
sub-diagrams show the planned trajectory at 4 risk levels; the planner is more conservative as
→
0
, and aligns with the results shown in Table I.
such that
n
,
N
obs
and
ν
n
,
√
1
−
n
n
. Lastly,
χ
≥
0
and
τ
∈
(0
,
1)
are the initial trust region and worst-case rate of
convergence, respectively.
Proof.
The
j
th
random bootstrapped obstacle forecasts can
be denoted as
z
j
i,k
,
(
α
j
i,k
)
T
x
i
+
β
j
i,k
, where
α
j
i,k
and
β
j
i,k
are defined in (12) and (13). We have shown that
the obstacle avoidance constraint (10) has an affine over
approximation (11), which is equivalently given by
z
j
i,k
<
0
.
Hence, the chance constraint (8d) is,
P
(
x
i
∈O
pred
) =
P
(
N
obs
⋃
k
=1
{
z
i,k
≥
0
}
)
≤
N
obs
∑
k
=1
P
(
z
i,k
≥
0)
.
Enforcing the chance constraints
P
(
z
i,k
≥
0)
≤
n
,
∀
k
∈
Z
1:
N
obs
also satisfies (8d). We can satisfy this chance
constraint in a distributionally robust manner:
sup
κ
∼
(
E
[
z
i,k
]
,
Σ
z
i,k
)
P
{
κ
≥
0
}≤
n
,
∀
i,k
∈
Z
1:
N
h
×
Z
1:
N
obs
,
where
E
[
z
i,k
]
and
Σ
z
i,k
are the sample mean and covari-
ance matrix of the bootstrapped
{
z
i,k
}
N
strap
1
. We reformulate
the above statement as a deterministic constraint as shown
in [21],
E
[
z
i,k
]
︸
︷︷
︸
E
[
α
i,k
]
T
C
x
i
+
E
[
β
i,k
]
+
ν
n
√
Σ
z
i,k
︸
︷︷
︸
∆
i,k
≤
0
,
∀
i
∈
Z
1:
N
h
,k
∈
Z
1:
N
obs
.
(18)
Constraint (18) is not affine in the optimization variable, as is
desirable for real-time application. By Lemma 2,
∆
i,k
≤
ζ
i,k
,
and we deduce the following tighter inequality constraint as
a numerically appealing alternative to (18),
E
[
α
i,k
]
T
C
x
i
+
E
[
β
i,k
]+
ν
n
(
1
T
|
Σ
1
/
2
α
i,k
p
i
−
h
i,k
|
+
√
3
k
i,k
)
≤
0
.
(19)
Cases
0
.
05
0
.
1
0
.
25
0
.
5
1
%
Feas.
97.5
98.2
98.9
99.6
100
Const.
%
Succ.
100
100
100
100
59.0
Speed
d
min
2.26
1.85
1.41
1.12
0.64
σ
(
d
min
)
0.42
0.33
0.25
0.22
0.35
%
Feas.
99.5
99.6
99.9
100
100
Ball
%
Succ.
100
100
100
100
79.3
w/drag
d
min
2.60
2.14
1.63
1.27
0.64
σ
(
d
min
)
1.08
0.93
0.70
0.50
0.27
%
Feas.
90.3
97.4
98.3
98.6
97.8
Frisbee
%
Succ.
100
100
100
100
58.0
w/drag
d
min
4.97
3.97
2.85
2.01
0.78
σ
(
d
min
)
1.97
1.53
1.15
0.91
0.77
TABLE I: Summary of results from Monte-Carlo simulations.
To account for the absolute value term, we introduce auxil-
iary optimization variables
s
i,k
that satisfy the following:
Σ
1
/
2
α
i,k
p
i
−
h
i,k
≤
s
i,k
,
−
Σ
1
/
2
α
i,k
p
i
+
h
i,k
≤
s
i,k
.
Therefore, satisfying (17d) is equivalent to satisfying (19).
Convergence of the SCP is proven in [22] which is based
on implementing a trust region via second-order cone con-
straints (17e). The authors also show the solution to the SCP
formulation as
w
→∞
feasibly solves problem 2.
5
VI. N
UMERICAL
E
XAMPLE
We consider a quadcopter that follows a reference trajectory
y
ref
while avoiding randomly generated moving obstacles
and adhering to state and control constraints. Let the position
of the quadcopter in frame
E
be
x,y,z
and the Euler angles
roll, pitch, and yaw are given by
φ,θ,ψ
respectively. The
following dynamic model is used in the simulation:
̈
x
=
−
9
.
81
θ,
̈
y
= 9
.
81
φ,
̈
z
=
−
u
1
−
9
.
81
,
̈
ψ
=
u
4
,
(20)
The planner control inputs are given by
u
1
,θ,φ,u
4
.
The reference trajectory consists of the desired positions,
{
x
ref
}
N
T
1
,
{
y
ref
}
N
T
1
,
{
z
ref
}
N
T
1
and yaw angles
{
ψ
ref
}
N
T
1
.
5
To be numerically feasible,
w
is usually upper bounded by a finite
integer, resulting in a sub-optimal but still feasible solution.
To demonstrate the effectiveness of the proposed method, we
conducted Monte-Carlo (MC) simulations of the proposed
planner avoiding three differently behaved obstacles which
are introduced once in each run. See the provided simulator
for details.
6
Case 1 is a constant speed spherical obstacle
without drag. Case 2 is a thrown spherical (ball) obstacle
with drag. Case 3 is a Frisbee that is thrown at various initial
angles, position, speed, and rotation speed. The spherical
obstacle dynamics are captured by a 6 state ODE with
drag penalties proportional to its velocities. The Frisbee is
modelled following [17], using a full 12 state model identical
to Fig.1 and aerodynamic drag coefficients.
We conduct 1000 MC simulations per
level to compare the
numerical feasibility, percent success in obstacle avoidance
(if the MPC planner is feasible), and the planner’s conserva-
tiveness, as measured by the minimum distance between the
obstacle and agent centers. For the three cases, the obstacle
speed ranges are
[0
.
41
,
8
.
43]
,
[3
.
41
,
6
.
37]
,
and
[5
.
76
,
6
.
68]
m/s, respectively. The MPC planning and measurement rates
are fixed to be 20 Hz. With a 10 step horizon length and 40
bootstraps, the average per planner update rate is
0
.
030
±
0
.
0014
sec solved using Gurobi [23] on an Intel i7-9700K
CPU @3.6GHz processor, dynamic simulation written in
MATLAB. The results in Table I show the applicability
of our SSA-MPC algorithm, despite vast differences in
obstacle behavior. Further, as the risk tolerance
shrinks, the
percentage success in obstacle avoidance (when the solution
is feasible) increases, with a trade-off in the feasibility of
optimization (17). The risk tolerance
can also viewed as
a robustness parameter which inversely proportional to the
distance between the agent and obstacles.
VII. C
ONCLUSION
Our data-driven risk-aware obstacle avoidance planner show-
cased near perfect results in avoiding moving obstacles with
limited and noisy measurements and no prior knowledge
about the obstacle behaviors. We not only offered a new
paradigm that can extract obstacle dynamics online allowing
short prediction, but an equally important risk-aware MPC
formulation that enables real-time usage. The simulation
result also shows that adjusting the risk level
can implicitly
adjust the safety distance between the agent and obstacles.
R
EFERENCES
[1] D. Fan, K. Otsu, Y. Kubo, A. Dixit, J. Burdick, and A.-A. Agha-
Mohammadi, “Step: Stochastic traversability evaluation and planning
for risk-aware off-road navigation,” in
Robot.: Sci. Syst.
, 2021.
[2] J. Latombe,
Robot motion planning
. Kluwer, 1996.
[3] J. Canny,
The complexity of robot motion planning
. MIT press, 1988.
[4] C. Tomlin, G. Pappas, and S. Sastry, “Conflict resolution for air
traffic management: a study in multiagent hybrid systems,”
IEEE
Transactions on Automatic Control
, vol. 43, no. 4, pp. 509–521, 1998.
[5] J. van den Berg, D. Ferguson, and J. Kuffner, “Anytime path planning
and replanning in dynamic environments,” in
IEEE Int. Conf. Robotics
and Automation
, pp. 2366–2371, 2006.
[6] C. Lam, C. C. K. Chiang, and L. Fu, “Human-centered robot naviga-
tion—towards a harmoniously human–robot coexisting environment,”
IEEE Trans. Robotics
, vol. 27, no. 1, pp. 99–112, 2011.
6
https://github.com/skylarXwei/Riskaware_MPC_SSA_Sim.git
[7] B. Lindqvist, S. Mansouri, A. Agha, and G. Nikolakopoulos, “Non-
linear mpc for collision avoidance and control of uavs with dynamic
obstacles,”
IEEE Robot.
&
Autom. Lett
, 2020.
[8] I. Hwang and C. Seah, “Intent-based probabilistic conflict detection
for the next generation air transportation system,”
Proceedings of the
IEEE
, vol. 96, no. 12, pp. 2040–2059, 2008.
[9] S. Brunton, B. Brunton, J. Proctor, E. Kaiser, and J. Kutz, “Chaos as
an intermittently forced linear system,”
Nat. commun.
, 2017.
[10] F. Takens, “Detecting strange attractors in turbulence,” in
Dynamical
systems and turbulence
, pp. 366–381, Springer, 1981.
[11] N. Goljandina and A. Zigljavskij,
Singular spectrum analysis for time
series
. Springer, 2020.
[12] N. Golyandina and A. Korobeynikov, “Basic singular spectrum anal-
ysis and forecasting with r,”
Comp. Stat.
&
Data Anal.
[13] J. Juang and R. Pappa, “An eigensystem realization algorithm for
modal parameter identification and model reduction,”
J. Guidance,
Control, and Dynamics
, vol. 8, no. 5, pp. 620–627, 1985.
[14] U. Fasel, J. Kutzand, B. Brunton, and S. Brunton, “Ensemble-SINDy:
Robust sparse model discovery in the low-data, high-noise limit, with
active learning and control,”
ArXiv
, Nov. 2021.
[15] D. Morgan, S. Chung, and F. Hadaegh, “Model predictive control of
swarms of spacecraft using sequential convex programming,”
Journal
of Guidance, Control, and Dynamics
, vol. 37, pp. 1–16, 04 2014.
[16] F. Augugliaro, A. Schoellig, and R. D’Andrea, “Generation of
collision-free trajectories for a quadrocopter fleet: A sequential convex
programming approach,”
Int. Conf Intell. Robots Syst.
, 2012.
[17] S. Hummel,
Frisbee flight simulation and throw biomechanics
. Uni-
versity of California, Davis, 2003.
[18] H. Adbi and L. Williams, “Principal component analysis,”
Wiley
interdisc. reviews: comp. statistics
, vol. 2, no. 4, pp. 433–459, 2010.
[19] N. Golyandina, V. Nekrutkin, and A. Zhigljavsky, “Analysis of time
series structure - ssa and related techniques,” in
Monographs on
statistics and applied probability
, 2001.
[20] A. Agarwal, A. Alomar, and D. Shah, “On multivariate singular
spectrum analysis,”
ArXiv
, 06 2020.
[21] L. Ghaoui, M. Oks, and F. Oustry, “Worst-case value-at-risk and robust
portfolio optimization: A conic programming approach,”
Operations
Research
, vol. 51, pp. 543–556, 08 2003.
[22] R. Foust, S. Chung, and F. Hadaegh, “Optimal guidance and control
with nonlinear dynamics using sequential convex programming,”
J. of
Guid., Control, and Dyn.
, vol. 43, no. 4, pp. 633–644, 2020.
[23] Gurobi Optimization, LLC, “Gurobi Optimizer Ref. Manual,” 2022.
APPENDIX
Proof.
Let the eigendecomposition of
Σ
α
i,k
be the fol-
lowing:
Σ
α
i,k
= [
U
r
U
n
]
[
Λ
r
0
0 0
]
[
U
r
U
n
]
T
where
U
r
∈
R
3
×
(3
−
n
i,k
)
is comprised of the eigenvectors of
Σ
α
i,k
that
are orthonormal. The columns of
U
n
∈
R
n
i,k
are the comple-
mentary orthonormal basis that spans the null space of
Σ
α
i,k
.
By substituting (16) one can verify the following inequality:
∆
i,k
≤
√
(
p
i
−
h
i,k
)
T
̃
Σ
α
i,k
(
p
i
−
h
i,k
)+
k
i,k
,
̃
∆
i,k
(21)
where
̃
Σ
α
i,k
is a positive definite matrix because
̃
Σ
α
i,k
= [
U
r
U
n
]
(
[
Λ
r
0
0 0
]
+
[
0
0
0
I
n
i,k
×
n
i,k
])
[
U
r
U
n
]
T
.
We further upper bound (21) by adding a positive constant,
ι
i,k
,
2
√
3
1
T
|
̃
Σ
1
/
2
α
i,k
p
k
−
h
k
|
, to
̃
∆
2
i,k
and obtain
̃
∆
2
i,k
≤
̃
∆
2
i,k
+
2
√
3
1
T
|
̃
Σ
1
/
2
α
i,k
(
p
k
−
h
k
)
|≤
√
ξ
T
i,k
ξ
i,k
where
ξ
i,k
,
|
̃
Σ
1
/
2
α
i,k
(
p
k
−
h
i,k
)
|
+
1
√
k
i,k
3
∈
R
3
. For the
inequality to hold, the expression
ι
i,k
must always be non-
negative which is true by construction. Further, let
ζ
i,k
=
1
T
ξ
i,k
∈
R
, then
ζ
2
i,k
= (
ξ
T
i,k
1
)(
1
T
ξ
i,k
) =
ξ
T
i,k
ξ
i,k
+ 2
ξ
. If
ξ
≥
0
, we can then state
∆
i,k
≤
ζ
i,k
which completes the
proof (since
ξ
i,k
= [
ξ
x
i,k
,ξ
y
i,k
,ξ
z
i,k
]
∈
R
3
, then
ξ
=
ξ
x
i,k
ξ
y
i,k
+
ξ
x
i,k
ξ
z
i,k
+
ξ
y
i,k
ξ
z
i,k
>
0
because
ξ
x
i,k
,ξ
y
i,k
,ξ
z
i,k
∈
R
+
).