Conference PaperPDF Available

Set-Based Predictive Control for Collision Detection and Evasion

Authors:
Set-Based Predictive Control for Collision Detection and Evasion
Jeremy Crowley, Yegeta Zeleke, Berk Altın and Ricardo G. Sanfelice
Abstract We propose a set-based predictive control frame-
work to predict inbound dynamic obstacles and optimize
trajectories in the interest of safely guiding a vehicle towards
a target. To account for uncertainties, the set-based controller
generalizes conventional model predictive control and predicts
the set that the state of a dynamical system might belong to.
This generalization is used to formulate collision avoidance as
a hard constraint in the set-based predictive control algorithm.
As a proof-of-concept, the proposed framework is applied to a
ground vehicle attempting to reach a target while anticipating
and evading collisions with obstacles in the operating environ-
ment. Other applications of the controller and the associated
optimal control problem are discussed.
I. INTRODUCTION
Recent research interest in autonomous vehicles for use
in civilian and military applications has been increasingly
ubiquitous. Modern autonomous vehicles are involved in nu-
merous applications ranging from rescue missions to package
deliveries. For many autonomous systems, model predictive
control (MPC) is preferred as a control strategy since col-
lision avoidance can be embedded into the constraints of
the corresponding optimal control problem, along with other
hard constraints such as actuator limitations. A supervisory
controller based on nonlinear MPC is used in [1] for a
pursuit-evasion game involving two fixed-wing autonomous
aircrafts. An MPC scheme that integrates tracking and sta-
bilization with a higher priority on collision avoidance was
discussed in [2]. In [3], nonstationary obstacles on the path
of an autonomous are avoided using MPC, where collision
avoidance is embedded as a soft constraint with high cost.
The control strategy in [3] is implemented in a hierarchical
fashion, where a high-level central MPC schemes supervises
low-level local MPC schemes. A numerical method for
aerial pursuit-evasion games is solved via MPC in [4]. Soft
constraints are also used in the MPC formulation of [5]
to ensure that a network of three autonomous helicopters
operate without collisions in a shared environment. A target
seeking scenario in an a priori unknown environment with
obstacles is realized via MPC in [6]. A predictive controller
is used in [7] for lane keeping and collision avoidance of an
autonomous ground vehicle. Recently, MPC was extended to
hybrid dynamical systems in [8], [9].
*Research partially supported by NSF Grants no. ECS-1710621 and CNS-
1544396, by AFOSR Grants no. FA9550-16-1-0015, FA9550-19-1-0053,
and Grant no. FA9550-19-1-0169, and by CITRIS and the Banatao Institute
at the University of California.
Jeremy Crowley is with the Department of Aeronautics and
Astronautics, Stanford University. Yegeta Zeleke, Berk Altın, and
Ricardo G. Sanfelice are with the Department of Electrical and
Computer Engineering, University of California, Santa Cruz, CA 95064,
USA; crowleyj@stanford.edu,yzeleke@ucsc.edu,
berkaltin@ucsc.edu,ricardo@ucsc.edu.
This paper proposes a novel collision detection and avoid-
ance strategy based on set-based predictive control. Building
from the so-called set dynamical systems framework in [10],
[11], [12], where the state of a system is identified by a
set rather than a point, the set-based predictive controller
generalizes conventional MPC along the lines of the tube-
based MPC approach [13]. Although collision detection
and evasion have been presented in [3],[7],[14] modeling
uncertainties that arise due to disturbances and measurement
noise has not been studied to the best of our knowledge.
In order to account for these unseen dynamical behaviors,
we propose a method by which a controller will predict
sequences of sets that the state of a discrete-time dynamical
system might belong to, and imposes constraints on the
predicted sets for robustness purposes.
Unlike other set-based approaches in the conventional
MPC literature (for example, [15], [16], [17]—see also
the survey [18] for the tube-based MPC technique), where
the optimal control problem is to minimize solutions of a
“classical” dynamical system (in the sense that the state is
identified by a point), the optimal control problem associated
with our strategy is to minimize solutions of a set dynamical
system. In other words, the minimization associated with
our strategy occurs over sequences of sets, rather than
points. In addition to collision detection and avoidance, the
proposed strategy can find use in various applications such
as uncertainty propagation, reachable set computation, and
safety analysis for conventional discrete-time systems. A
detailed comparison of the proposed strategy with existing
collision and avoidance schemes is outside the scope of this
exploratory work, and will be tackled in the future.
The rest of the paper is organized as follows. Section II
outlines the collision detection and evasion problem and
details our proposed solution based on set-based predictive
control. As a proof-of-concept, in Section III, simulation
results for the proposed control algorithm are validated
via simulations, followed by experimental results for an
autonomous ground vehicle seeking a target in the presence
of obstacles. Concluding remarks are given in Section IV.
Notation: We use Rto represent real numbers and R0
its nonnegative subset. The set of nonnegative integers is
denoted N. The 2-norm is denoted |.|. The distance of a vec-
tor xRnto a nonempty set A ⊂ Rnis |x|A:= infa∈A |x
a|. Let X, Y Rn. The notation XYindicates that X
is a subset of Y, not necessarily proper. The Minkowski
sum of Xand Yis denoted X+Y. The notation P(X)
denotes the set of nonempty subsets of X.Pn
pdenotes the
set of compact convex polytopes in Rnwith pvertices.
For any xRnand yRm,(x, y) := x>y>>. Given
a (set-valued) mapping Fand a set Xin its domain, we
use F(X)to denote the image of Xunder F.
II. SE T-BAS ED PREDICTIVE CON TROL
Consider the discrete-time dynamical system
x+=f(x, u, w)(1)
with state xRn, input uRm, and disturbance wRl,
where f:Rn×Rm×RlRn. In (1), x+denotes the value
of the state xafter a discrete transition under the input uand
disturbance w. In the presence of measurement noise, at any
discrete time jN, the value of the state xjis not known
with certainty, but can be estimated to belong to a set Xj. For
example, if the additive noise vjRnsatisfies |vj| ≤ δ, then
given the state measurement ˆxj=xj+vj, we have xjXj,
where Xj={xRn:|xˆxj| ≤ δ}. More generally,
suppose that wjWand (vj)Vfor some sets W
and V, at every jN. By (1),
xj+1 =fxjvj, uj, wj)f(ˆxj+V, uj, W )
=f(Xj, uj, W ) =: F(Xj, uj)(2)
where F:Rn×RmRnis a set-valued mapping.
Set-based approaches along the lines of the difference
inclusion x+F(x, u)are common in the robust MPC
literature [13], where the optimal control problem is designed
to constrain trajectories (or solutions) x0, x1, . . . , xNto se-
quences of sets X0, X1, . . . , XN, or “tubes”, so that xjXj
for all j∈ {0,1, . . . , N }. As a key difference with those
approaches, in this paper, we deal directly with trajectories
given by sequences of sets X0, X1, . . . , that are not neces-
sarily tubes [10], [11], [12].
A. The Predictive Control Problem
We propose a set-based predictive control scheme for
discrete-time systems with solutions given by sequences of
sets. Given the difference inclusion x+F(x, u)model-
ing the system to control, which can arise from factoring
in uncertainties in (1), instead of predicting sequences of
points x0, x1, . . . , xNRnand selecting the sequence
with the least cost, we propose to predict sequences of
sets X0, X1, . . . , XNRnand select the sequence with
the least cost for the set dynamical system
X+=F(X, U ).(3)
As in standard MPC, the ingredients of this controller
include a prediction horizon N1, a control horizon M
with 1MN, a stage cost L:P(Rn×Rm)R0,
a terminal cost V:P(Rn)R0, a mixed-constraint
set C ⊂ P(Rn×Rm), and a terminal constraint set XV
P(Rn). Note that L(respectively, V) assigns a cost to
every nonempty subset of Rn×Rm(respectively, Rn) as
opposed to the formulation in [13]. Similarly, the constraint
set C(respectively, XV) is a collection of nonempty subsets
of Rn×Rm(respectively, Rn).
The proposed controller operates by solving the following
problem to predict input sets U?
0, U ?
1, . . . , U ?
N1such that the
solution X?
0, X?
1, . . . , X?
Nof the set dynamical system (3)
minimizes the cost in (4), subject to constraints.
Problem 2.1: Given the prediction horizon N, stage
cost L, terminal cost V, constraint sets Cand XV, and
initial condition set X0, find sequences X?:= {X?
j}N
j=0
and U?:= {U?
j}N1
j=0 minimizing the cost
J(X?, U ?) :=
N1
X
j=0
L(X?
j, U ?
j)
+V(X?
N),(4)
subject to the constraints X?
0=X0and
X?
j+1 =F(X?
j, U ?
j)j∈ {0,1, . . . , N 1}
(X?
j, U ?
j)∈ C j∈ {0,1, . . . , N 1}
X?
NXV.
The optimal input sequence is applied until time step M,
at which point the process is repeated for the new initial con-
dition. That is, the sequence U?
0, U ?
1, . . . , U ?
M1is applied to
obtain the solution X?
0, X?
1, . . . , X?
M, and then Problem 2.1
is re-solved by setting the initial condition set X0=X?
M.
This process is summarized in Algorithm 1, where iis the
time step of the closed loop and b
X:= {b
Xi}
i=0 is the
resulting closed-loop state trajectory.
Algorithm 1: Set-based predictive control.
1Obtain initial system state b
X0, set i= 0 and X0=b
X0
2while True do
3Solve Problem 2.1
4j= 0
5for jM1do
6b
Xi+1 =X?
j+1 =F(X?
j, U ?
j)
7i=i+ 1,j=j+ 1
8end
9Set X0=X?
M
10 end
Remark 2.2: In tube-based MPC, the minimization is per-
formed over nominal point-based trajectories, and the con-
straints are defined using subsets of Rnand Rm. To ensure
that constraints are satisfied under uncertainties, an auxiliary
controller is employed to steer trajectories towards that of the
nominal system. In contrast, the minimization in Problem 2.1
is performed directly over set-based trajectories, and the
constraints are defined using power sets of Rnand Rm.
As such, the proposed framework differs considerably from
tube-based MPC and similar set-based methodologies.
B. Applications
The formulation in (2) shows that the set dynamical
system (3) can be used to compute uncertainty propagation
in the discrete-time system (1). In addition, as noted in [10],
the set dynamical system in (3) arises in a wide range of
problems. Below, we discuss three example applications.
1) Reachable Set Computation: The system in (3) can
be used to compute the reachable set of (1) from an initial
set X0up to timestep Nif F(X, U ) = f(X, U, 0). To
compute reachable sets using Problem 2.1, the stage cost
and terminal cost can be selected such that
L(X, U ) = V(X) = a
1 + bRRnıX(x)dx (5)
for some a, b > 0, where ıXis the indicator function of X.
Roughly speaking, L(X, U ) = V(X)a > 0is inversely
proportional to the volume of X, so minimizing the cost
in (4) leads to larger sets. The constraint sets can be selected
as C=P(X × U)and X?
N=P(X)for some X Rn
and U Rm, representing the state and the input constraints,
respectively. To make this problem tractable, it would be
necessary that U 6=Rm. When the initial set X0has
no volume, the integral in (5) can be replaced with other
mappings measuring the size of X.
2) Safety Analysis: When the mapping Fin (3) is derived
via (2), a robust safety analysis for (1) can be conducted by
checking whether given a closed safe set Kand K0K,
there exists a sequence of inputs U0, U1, . . . such that the
corresponding solution X0, X1, . . . of (3) satisfies XjK
for all jNwhen X0=K0. If the state and the input
constraints can again be represented by some sets X Rn
and U Rm, safety analysis can be conducted by repeatedly
solving Problem 2.1 as in Algorithm 1, with C=P(X ×
U),XV=P(Rn)and L(X, U ) = V(X) = supxX|x|K.
If K0=Kand U={0}, this problem reduces to the forward
invariance problem for the inclusion x+F(x, 0).
3) Collision Avoidance: An autonomous vehicle with the
dynamics in (1) that is trying to reach a target set X
while avoding obstacles can be controlled by the predictive
control strategy of Algorithm 1. To ensure convergence to
the target, the cost functions Land Vcan be designed such
that L(X, U ) = V(X) = 0 if XX. Obstacle avoidance
can be ensured by choosing Cto be a subset of P(X ×U)for
a state constraint set Xand input constraint set U. Next, we
detail the application of Algorithm 1 to collision avoidance.
C. Collision Detection and Evasion for Autonomous Vehicles
We consider a scenario in which a vehicle with the
dynamics in (1) has to avoid a stationary obstacle represented
by a set Yand reach a target set X. We assume that the
dynamics in (1) are overapproximated by the set-based dy-
namics in (3), where the set state Xand input Uare compact
convex polytopes, and Fmaps compact convex polytopes
to compact convex polytopes. The choice of polytopic sets
are meant to facilitate computations, especially in the case
where the mapping Fis affine and single valued: since affine
transformations map vertices to vertices, the set F(X, U )is
precisely the convex hull of F(XV, UV), where XVand UV
are the set of vertices of Xand U, respectively. Such a
scenario can arise when the mapping fis affine and the
set Win (2) is also taken to be a compact convex polytope.
In the case where the mapping fin (1) is not affine, Fcan
be nevertheless be chosen such that F(X, U )f(X, U)is
a compact convex polytope.
Remark 2.3: A similar set-based approach to motion plan-
ning is proposed in [19], where the focus is on the reachable
set computation of nonlinear systems under disturbances,
with initial conditions belonging to zonotopic sets.
1) Selecting the Cost Functions and the Terminal Con-
straint: To ensure that the vehicle can reach the target X
in the absence of collisions, the cost functions Land Vcan
be designed to have distance-like properties. The terminal
constraint set XVshould be selected so that XVXis
nonempty, as the converse would prevent the vehicle from
reaching Xwhen M=N—this is similar to conventional
MPC, where Xwould be the origin, and XVwould be a
neighborhood of the origin. For simplicity, we let L(X, U ) =
Pp
i=1 |xi|Xand V(X) = cPp
i=1 |xi|Xfor some c > 0,
where x0, x1, . . . , xpare the vertices of X.
2) Encoding Safety Constraints into C:The mixed con-
straint set Ccan be utilized to prevent collisions between
the vehicle and the obstacle. The state could be subject
to X⊂ X, where Xis the state constraint set, and the
input could be subject to U⊂ U, where Uis the input
constraint set. The set Xtypically models the geometry of
the operating environment of the vehicle-obstacle system,
while the input constraint set Ucould arise due to actuator
limitations like saturation. Denote by Han output mapping
so that collisions correspond to H(X)intersecting Y. Then,
the mixed constraint set can be selected as
C={X×U⊂ X × U :σ(H(X), Y )ς}
Pn
px×Pm
pub
C,(6)
for some ς > 0and px, puN. Above, the map-
ping σ(H(X), Y ) := inf (x,y )X×Y|H(x)y|is a measure
of the shortest path between the sets H(X)and Y, and the
generic set b
C ⊂ P(Rn×Rm)can be used to encode other
constraints. In most cases, Hwould be a linear function
extracting the position coordinates.
Other safety constraints can also be encoded in C. In
particular, the trajectory of the vehicle in the intersample
period can be approximated by con(XF(X, U )), the
convex hull of Xand F(X, U), and the term H(X)in (6)
can be replaced with H(con (XF(X, U ))).
D. Challenges of Problem 2.1 and Algorithm 1
From a computational standpoint, there are several chal-
lenges associated with Problem 2.1 and Algorithm 1.
(C1) The presence of state and input sets, along with the
constraints, prevents the use of standard techniques
to solve Problem 2.1, making its solution difficult in
general.
(C2) The computational burden associated with a numerical
solution of Problem 2.1 can be high enough to prevent
online implementation.
(C3) Perturbations on the set dynamical system (e.g. delays,
unmodeled dynamics) can adversely affect the perfor-
mance of Algorithm 1 and lead to constraint violations.
In essence, the above challenges are similar to those
arising in the context of conventional optimal control and
MPC. Although Problem 2.1 is somewhat conceptual, it
can be solved in certain cases. For example, in the case of
the scenario described in Section II-C, Problem 2.1 can be
solved using nonlinear programming methods. In general,
Problem 2.1 can be solved suboptimally with acceptable
computational burden. The amount of computational burden
deemed acceptable would naturally depend on the applica-
tion. For online implementation of Algorithm 1, if (3) is
derived from continuous-time dynamics, the time to compute
should to be reasonably smaller than the sampling period.
These challenges are discussed further in Section III. In
particular, as a proof-of-concept, we show how Algorithm 1
can be used in an applied setting, demonstrating the value
of the proposed framework. The simulations in Section III
show how (C1)-(C2) can be addressed by careful selection
of the constraints and costs. To address (C2)-(C3) and show
that Algorithm 1 can tolerate perturbations while running
fast enough for online implementation, experiments are con-
ducted in Section III-D, with the results in Section III-C
forming a baseline for comparison.
III. APP LI C ATION TO COLLISION DET ECT IO N A ND
EVASION F OR VEHICLES
We now show how the set-based predictive control scheme
outlined in Section II can be effectively applied to a ground
vehicle in the presence of static and dynamic obstacles.
A. Vehicle and Obstacle Dynamics
We model our ground vehicle using the Dubins model
˙q1=vcos(θ),
˙q2=vsin(θ),
˙
θ= (v/L) tan(φ) =:ω,
(7)
where q1and q2are the Cartesian coordinates, θis the
heading angle, vis the speed, Lis the length of the vehicle,
and φis the steering angle. The exact discretization of (7)
with the step size Tyields the model
x+=f(x, u, w) =
q1+u12 cos(θ+u2) sin(u2)
ω
q2+u12 sin(θ+u2) sin(u2)
ω
θ+ 2u2
,
where x:= (q1, q2, θ)and u:= (u1, u2)=(v, T ω /2).
Although the system in (3) allows for the explicit inclusion
of disturbances, for simplicity, the set dynamical system
which we will use for prediction purposes will be given
by the mapping F(X, U ) = f(X, U, 0), and the effects of
disturbances on (7) will be embedded into the set state X.
B. Constraint Selection
For simplicity, we do not impose any state constraints on
the vehicle, i.e., X=R3. Similarly, XV=R3. The target
set is taken as X={−0.50}×{0.06} × R, and the input
constraint set Uin (6) is taken as
U={(u1, u2) : 0 u1β, (T α)/2u2(T α)/2}.
Here, β0and α0limit the magnitude of the vehicle’s
speed in m/s and steering angle in radians, respectively.
For Problem 2.1 to be computationally viable for real-
time implementation, a concern raised in (C2), inputs for
set dynamics, as in (3), are taken to be singletons. That is,
for each j∈ {0,1, . . . , N 1}, the decision variable U?
jof
Problem 2.1 is chosen from subsets of Rpconsisting of a
single element, i.e., we only consider the case where pu= 1
in (6). Furthermore, we assume a scenario where the state
components (q1, q2)are subject to uncertainty, while θcan
be measured exactly. More specifically, we assume that
x[z1, z2]×[z3, z4]× {z5} ⊂ R×R×R(8)
for some z1,z2,z3,z4,z5. The rectangular set in (8) cor-
responds to the translation of the set Vin (2). To formalize
this in (6), we take px= 4 and b
C=P(R2)×R× P(R),
as n= 3 and m= 1; i.e. any (X, U )∈ C should be such that
the projection of Xonto the θspace should be a singleton.
The output mapping His chosen as a linear mapping such
that H([z1, z2]×[z3, z4]× {z5})=[z1, z2]×[z3, z4].
The selection of the constraints with this structure ensures
that the set F(X, U )is of the same size as X(in terms
of its area in the (q1, q2)space), and prevents a scenario
where the resulting system is “uncontrollable”. The choice of
rectangular sets further reduces the computational burden of
computing the propagation of the polytope in (8), which can
be represented by a matrix in R3×4(or equivalently a vector
in R12): since the dynamics of q1and q2are decoupled,
letting z:= (z1, z2, z3, z4, z5), where zi’s come from (8),
for prediction purposes, we rely on the discrete-time model
z+=z+
T u12 cos(z5+u2) sin(u2)
u2
T u12 cos(z5+u2) sin(u2)
u2
T u12 sin(z5+u2) sin(u2)
u2
T u12 sin(z5+u2) sin(u2)
u2
2u2
.(9)
Section II-C outlines the safety constraint, which requires
the nontrivial computation of σ(H(X), Y )in (6). This can be
accomplished by minimizing |P1λ1P2λ2|2via a quadratic
program, where, for each i∈ {1,2},PiRn×pirepresents
a polytope with each column corresponding to a vertex,
and λiRpiis subject to constraints such that Piλi
represents convex combinations.
C. Nominal Results
The simulations1, conducted on a MacBook Pro com-
puter with 2.5 GHz Intel core i5 processor and 8GB RAM,
were run with the step size T= 0.05 s, N=8,
M=2, ς=0.05 m. The parameters for the input con-
straints were chosen as β= 0.78 and α=π/6. The
simulations assume a 2.5 m×3.0 m operating environment,
with a rectangular obstacle with dimensions 0.05 m×0.05 m
centered at (0.025,0.083). The size of the set used for the
vehicle is the same size as the vehicle used in the upcom-
ing experiments, namely 0.451 m×0.331 m. As opposed to
solving the quadratic program discussed in Section (III-B),
1github.com/HybridSystemsLab/SetBasedPredictionCollisionAndEvasion
to address (C2), we underapproximate the minimum dis-
tance between the two rectangular polytopes with the for-
mula max{|c1c2| − (r1+r2),0}, where ciis the center
coordinate of polytope iand riis the distance from cito a
vertex of polytope i, corresponding to the distance between
circular overapproximations centered at xiwith radius ri. For
180 optimization tasks, the resulting runtime of the modified
algorithm has an average of 0.1008 seconds with standard
deviation of 0.0558 seconds. The minimum and maximum
runtimes are 0.0361 and 0.3607 seconds, respectively.
-1.5 -1 -0.5 0 0.5 1
-1.5
-1
-0.5
0
0.5
1
1.5
q2[m]
q1[m]
Fig. 1: Simulation results from nine initial conditions.
Figure 1 shows that the algorithm is successful in avoiding
the obstacle and converges to a region near the target ().
For each trajectory, the set of states in the same prediction
horizon use the same color. The prediction horizons start
colored in blue and gradually transitions to red. It can be seen
in Figure 2 that the safety constraints are satisfied at all times.
The dashed line at the top subfigure depicts ςand the tra-
jectories converge to a small neighborhood. The dashed line
at the bottom subfigure depicts Dmin = 2(L2+W2) =
1.1184, where Lis the length and Wis the width of the
rectangle polytope.
0 4 8 12 16 20
0
0.3
0.6
0.9
1.2
0 4 8 12 16 20
0
1.5
3
4.5
6
Distance [m]
t[s]
Fig. 2: Top: Minimum distance from polytope to obtacle.
Bottom: Sum of distances between vertices and obstacle.
To fully use the predictive qualities of this algorithm,
simulations were run with a dynamic obstacle from four
different initial conditions. The obstacle state was subject to
the same polytopic constraints as the vehicle, evolving under
the effect of a known constant input with 0u1β. As
such, the model in (9) was used to predict the motion of the
obstacle. The parameters used for these simulations were the
same as the static obstacle case except for the target, which
was taken as X={0}×{0} × R. Figure 3 shows that the
vehicle can successfully converge to a neighborhood of the
target while avoiding collisions.
0 5 10 15 20 25
0
0.4
0.8
1.2
1.6
0 5 10 15 20 25
0
2.5
5
7.5
10
Distance [m]
t[s]
Fig. 3: Dynamic obstacle simulations data, as in Figure 2.
D. Experimental Results
To demonstrate the applicability of Algorithm 1 in a
real-world setting, experiments reproducing the simulation
scenarios with a stationary obstacle were carried out. In
particular, we show that despite the challenges outlined
in (C1)-(C3), Algorithm 1 guides the vehicle toward the
target while avoiding collision with the obstacle. In addi-
tion, we discuss the effects of uncertainties arising from
computational delays, the simplicity of the employed model
in (7), and quantify the time-to-compute the control input for
Problem 2.1.
1) Experimental setup: The considered experimental sys-
tem is a multi-node robotic system comprising a Windows
computer with Intel i5 dual core (3.20 and 3.19 GHz)
processor and 8 GB RAM, a motion capture system, a
radio-frequency (RF) communication system, and a radio-
controlled (RC) vehicle (communication up to 2.4 GHz).
Data from eight Flex-13 cameras are transferred to MATLAB
with the OptiTrack motion-capture software. A near real-time
communication between MATLAB and the motion capture
hardware is managed using the OptiTrack NatNet library,
yielding a minimal communication latency of 8.3 ms.
2) Analysis of Experimental Results: A series of initial
conditions are considered to confirm the simulation results
in Section III-C (Figure 4). As in Figure 1, each initial
condition is marked by a 6 edged star, the obstacle is
marked by a red rectangle, and the target is marked by black
star. Continuous red lines show the phase portraits of the
vehicle trajectories. As in Figure 1, the set of states in the
same prediction horizon are colored similarly for each run.
From the latter color coding, one can observe the existence
of a gap between consecutive set states at certain times
due to computational delays, a phenomenon not observed
in the simulations. Nevertheless, despite delays and model
uncertainties, the vehicle successfully avoids the obstacle and
reaches a neighborhood of the target.
-1.5 -1 -0.5 0 0.5 1
-1.5
-1
-0.5
0
0.5
1
1.5
q2[m]
q1[m]
Fig. 4: Experimental results corresponding to Figure 1.
0.4 0.8 1.2 1.6
0.03
0.06
0.09
0.12
tc[s]
d[m]
Fig. 5: Mean (circle) and median (star) of computational
time.
The dependency of the computational delay on the dis-
tance between the vehicle and obstacle is illustrated in
Figure 5. That is, the predicted trajectories illustrated in
Figure 4 are categorized according to the initial distance
from the vehicle to the obstacle. For example, all predicted
trajectories where the vehicle-distance is less than 0.4 m
belong to the same category. For each category, we gather all
the computational latency and report the mean and median
on Figure 5. As it can be seen from there, the computation
time increases as the vehicle gets closer to the obstacle. The
median computation time is approximated by the polynomial
(the blue dashed line) tc(d) = 0.07(d)3+ 0.26(d)2
0.32(d)+0.15, where ddenotes the distance from the vehicle
to the obstacle.
Despite some performance degradation, the results illus-
trate the effectiveness of the proposed algorithm on an
experimental platform in the presence of the challenges listed
in (C1)-(C3). This motivates the use of set-based predictive
control for motion planning and control in mobile robotics,
as well as the development of formal stability guarantees for
Algorithm 1 and numerical tools to solve Problem 2.1.
IV. CONCLUSION
This paper presented a set-based predictive control ap-
proach to collision avoidance and path planning, derived
from the extension of optimal control problems in MPC to
set dynamical systems, to account for uncertainties. Simula-
tion and experimental results show that the optimal control
problem can be solved suboptimally to reduce computational
burden, and in doing so, guide an autonomous vehicle safely
towards a target. Future work will focus on the development
of numerical tools and formal feasibility/stability guarantees.
REFERENCES
[1] J. M. Eklund, J. Sprinkle, and S. S. Sastry, “Switched and symmetric
pursuit/evasion games using online model predictive control with
application to autonomous aircraft,” IEEE Transactions on Control
Systems Technology, vol. 20, no. 3, pp. 604–620, 2012.
[2] J. Funke, M. Brown, S. M. Erlien, and J. C. Gerdes, “Collision
avoidance and stabilization for autonomous vehicles in emergency
scenarios,” IEEE Transactions on Control Systems Technology, vol. 25,
no. 4, pp. 1204–1216, 2017.
[3] Y. Gao, T. Lin, F. Borrelli, E. Tseng, and D. Hrovat, “Predictive control
of autonomous ground vehicles with obstacle avoidance on slippery
roads,” in ASME 2010 dynamic systems and control conference, vol. 1.
American Society of Mechanical Engineers, 2010, pp. 265–272.
[4] S. Kang, H. J. Kim, and M.-J. Tahk, “Aerial pursuit-evasion game
using nonlinear model predictive guidance,” in AIAA Guidance, Nav-
igation, and Control Conference, 2010, pp. 1–12.
[5] H. J. Kim, D. H. Shim, and S. Sastry, “Nonlinear model predictive
tracking control for rotorcraft-based unmanned aerial vehicles,” in
American Control Conference, 2002. Proceedings of the 2002, vol. 5.
IEEE, 2002, pp. 3576–3581.
[6] J. Liu, P. Jayakumar, J. Stein, and T. Ersal, “Combined speed and
steering control in high speed autonomous ground vehicles for obstacle
avoidance using model predictive control,IEEE Transactions on
Vehicular Technology, 2017.
[7] A. Carvalho, Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “Pre-
dictive control of an autonomous ground vehicle using an iterative
linearization approach,” in Intelligent Transportation Systems-(ITSC),
2013 16th International IEEE Conference on. IEEE, 2013, pp. 2335–
2340.
[8] B. Altın, P. Ojaghi, and R. G. Sanfelice, “A model predictive control
framework for hybrid dynamical system,” in 6th IFAC Conference on
Nonlinear Model Predictive Control NMPC 2018, 8 2018, pp. 128 –
133.
[9] B. Altın and R. G. Sanfelice, “Asymptotically Stabilizing Model
Predictive Control for Hybrid Dynamical Systems,” in American
Control Conference (ACC), 2019, 7 2019, p. to appear.
[10] R. G. Sanfelice, “Asymptotic properties of solutions to set dynamical
systems,” in 53rd IEEE Conference on Decision and Control, Dec
2014, pp. 2287–2292.
[11] N. Risso and R. G. Sanfelice, “Detectability and invariance properties
for set dynamical systems,” IFAC-PapersOnLine, vol. 49, no. 18, pp.
1030 – 1035, 2016, 10th IFAC Symposium on Nonlinear Control
Systems NOLCOS 2016.
[12] ——, “Sufficient conditions for asymptotic stability and feedback con-
trol of set dynamical systems,” in 2017 American Control Conference
(ACC), May 2017, pp. 1923–1928.
[13] S. V. Rakovi´
c and D. Q. Mayne, “Robust model predictive control
for obstacle avoidance: discrete time case,” in Assessment and Future
Directions of Nonlinear Model Predictive Control. Springer, 2007,
pp. 617–627.
[14] Z. Chao, L. Ming, Z. Shaolei, and Z. Wenguang, “Collision-free UAV
formation flight control based on nonlinear MPC,” in Electronics,
Communications and Control (ICECC), 2011 International Confer-
ence on. IEEE, 2011, pp. 1951–1956.
[15] A. Gautam, Y. C. Soh, and Y. Chu, “Set-based model predictive
consensus under bounded additive disturbances,” in 2013 American
Control Conference, June 2013, pp. 6157–6162.
[16] G. A. Gonalves and M. Guay, “Robust discrete-time set-based adaptive
predictive control for nonlinear systems,Journal of Process Control,
vol. 39, pp. 111 – 122, 2016.
[17] H. H. Nguyen, A. Savchenko, S. Yu, and R. Findeisen, “Improved
robust predictive control for Lure systems using set-based learning,
IFAC-PapersOnLine, p. to appear, 2018, 6th IFAC Conference on
Nonlinear Model Predictive Control NMPC 2018.
[18] D. Q. Mayne, “Model predictive control: Recent developments and
future promise,” Automatica, vol. 50, no. 12, pp. 2967 – 2986, 2014.
[19] B. Schrmann and M. Althoff, “Guaranteeing constraints of disturbed
nonlinear systems using set-based optimal control in generator space,”
IFAC-PapersOnLine, vol. 50, no. 1, pp. 11515 – 11 522, 2017, 20th
IFAC World Congress.
Article
This article focuses on distributionally robust controller design for safe navigation in the presence of dynamic and stochastic obstacles, where the true probability distributions associated with the disturbances are unknown. Although the true probability distributions are considered to be unknown, they are considered to belong to a set of probability distributions known as the ambiguity set. This ambiguity set includes all the probability distributions that share the same first two moments. In this article, the safe navigation problem has been defined by an optimal control problem with probabilistic collision avoidance constraint. To ensure satisfaction of this probabilistic constraint in the presence of disturbances whose true probability distributions are known, this constraint has been enforced in a distributionally robust sense. A computationally tractable control approach has been presented in this article that exploits techniques from robust optimization methods. Simulation results show the effectiveness of the proposed method.
Full-text available
Article
We address the problem of finding an optimal solution for a nonlinear system for a set of initial states rather than just for a single initial state. In addition, we consider state and input constraints as well as a set of possible disturbances. While previous optimal control techniques typically ignore the fact that the current state of a system is not exactly known, future safety-critical systems demand that all uncertainties including the initial state are considered; this is required for e.g. automated vehicles, surgical robots, or human-robot interaction. We present a new method that obtains optimal control inputs by finding optimal weights for generators that span the space reachable by the considered system. This solution routine can be used not only for a single initial state but also for a set of initial states - this is not possible using classical optimization techniques. We ensure that all constraints are met by using reachability analysis, which provides formal bounds for all possible system trajectories. We demonstrate the applicability of our approach with an example from automated driving; for this example, the result is obtained within a few seconds and outperforms a classical LQR approach.
Full-text available
Conference Paper
This paper presents the design of a controller for an autonomous ground vehicle. The goal is to track the lane centerline while avoiding collisions with obstacles. A nonlinear model predictive control (MPC) framework is used where the control inputs are the front steering angle and the braking torques at the four wheels. The focus of this work is on the development of a tailored algorithm for solving the nonlinear MPC problem. Hardware-in-the-loop simulations with the proposed algorithm show a reduction in the computational time as compared to general purpose nonlinear solvers. Experimental tests on a passenger vehicle at high speeds on low friction road surfaces show the effectiveness of the proposed algorithm.
Conference Paper
Predictive control of uncertain nonlinear systems is challenging. Existing approaches often require to find a global minima of a nonconvex optimization problem, and often are conservative, as the worst case solution is considered. This paper presents a robust model predictive control scheme for Lur'e systems subject to constraints, which improves via learning over time and allows efficient implementation using Linear Matrix Inequalities. The approach utilizes Lipschitz continuity conditions for the unknown sector bounded nonlinearity. Based on reconstructions of the unknown function from past experiments and measurements, the bound on the uncertainty is improved - learned - in an set-based way. The system is controlled by a continuous time linear feedback law, where the feedback matrix used is updated in a sampled data fashion solving an infinite horizon robust control problem that guarantees constraint satisfaction and robust stability. To improve the performance, constraints based on the learned data are added to the LMI formulation, which allows to guarantee stability and satisfaction of input and state constraints. Due to convexity of the resulting LMI formulation its computational demand is low, allowing to implement the method on systems with limited computational capabilities. The effectiveness of the approach is illustrated by an example of a flexible link robotic arm.
Article
This paper presents a model predictive control-based obstacle avoidance algorithm for autonomous ground vehicles at high speed in unstructured environments. The novelty of the algorithm is its capability to control the vehicle to avoid obstacles at high speed taking into account dynamical safety constraints through a simultaneous optimization of reference speed and steering angle without a priori knowledge about the environment and without a reference trajectory to follow. Previous work in this specific context optimized only the steering command. In this work, obstacles are detected using a planar light detection and ranging sensor. A multi-phase optimal control problem is then formulated to simultaneously optimize the reference speed and steering commands within the detection range. Vehicle acceleration capability as a function of speed, as well as stability and handling concerns such as preventing wheel lift-off are included as constraints in the optimization problem, whereas the cost function is formulated to navigate the vehicle as quickly as possible with smooth control commands. Simulation results show that the proposed algorithm is capable of safely exploiting the dynamic limits of the vehicle while navigating the vehicle through sensed obstacles of different size and number. It is also shown that the proposed variable speed formulation can significantly improve performance by allowing navigation of obstacle fields that would otherwise not be cleared with steering control alone.
Article
Invariance properties and convergence of solutions of set dynamical systems are studied. Using a framework for systems with set-valued states, notions of stability and detectability, similar to the existing results for classical dynamical systems, are defined and used to obtain information about the convergence properties of solutions. In particular, it is shown that local stability, detectability, and boundedness can be combined to conclude convergence of set-valued solutions. Under the assumption of bounded solutions and outer semicontinuity of the set-valued maps that define the system’s dynamics, invariance properties for set dynamical systems are also presented along with an invariance principle. The invariance principle involves the use of Lyapunov-like functions to locate invariant sets. Examples illustrate the results.
Article
Emergency scenarios may necessitate autonomous vehicle maneuvers up to their handling limits in order to avoid collisions. In these scenarios, vehicle stabilization becomes important to ensure that the vehicle does not lose control. However, stabilization actions may conflict with those necessary for collision avoidance, potentially leading to a collision. This paper presents a new control structure that integrates path tracking, vehicle stabilization, and collision avoidance and mediates among these sometimes conflicting objectives by prioritizing collision avoidance. It can even temporarily violate vehicle stabilization criteria if needed to avoid a collision. The framework is implemented using model predictive and feedback controllers. Incorporating tire nonlinearities into the model allows the controller to use all of the vehicle's performance capability to meet the objectives. A prediction horizon comprised of variable length time steps integrates the different time scales associated with stabilization and collision avoidance. Experimental data from an autonomous vehicle demonstrate the controller safely driving at the vehicle's handling limits and avoiding an obstacle suddenly introduced in the middle of a turn.
Article
The problem of robust adaptive predictive control for a class of discrete-time nonlinear systems is considered. First, a parameter estimation technique, based on an uncertainty set estimation, is formulated. This technique is able to provide robust performance for nonlinear systems subject to exogenous variables. Second, an adaptive MPC is developed to use the uncertainty estimation in a framework of min–max robust control. A Lipschitz-based approach, which provides a conservative approximation for the min–max problem, is used to solve the control problem, retaining the computational complexity of nominal MPC formulations and the robustness of the min–max approach. Finally, the set-based estimation algorithm and the robust predictive controller are successfully applied in two case studies. The first one is the control of anonisothermal CSTR governed by the van de Vusse reaction. Concentration and temperature regulation is considered with the simultaneous estimation of the frequency (or pre-exponential) factors of the Arrhenius equation. In the second example, a biomedical model for chemotherapy control is simulated using control actions provided by the proposed algorithm. The methods for estimation and control were tested using different disturbances scenarios.
Article
Dynamical systems with trajectories given by sequences of sets are studied. For this class of generalized systems, notions of solution, invariance, and omega limit sets are defined. The structural properties of omega limit sets are revealed. In particular, it is shown that for complete and bounded solutions, the omega limit set of a bounded and complete solution is nonempty, compact, and invariant. Lyapunov-like conditions to locate omega limit sets are also derived. Tools from the theory of set convergence are conveniently used to prove the results. The findings are illustrated in several examples and applications, including the computation of reachable sets and forward invariant sets, as well as in propagation of uncertainty.