九州大学学術情報リポジトリ
Kyushu University Institutional Repository
Embedded nonlinear model predictive control for
obstacle avoidance using PANOC
Sathya, Ajay
Department of Mechanical Engineering, MECO research group, KU Leuven
Sopasakis, Pantelis
Department of Electrical Engineering (ESAT-STADIUS), KU Leuven
Ruben Van Parys
Dept. of Mechanical Engineering, MECO research group, KU Leuven
Themelis, Andreas
Department of Electrical Engineering (ESAT-STADIUS), KU Leuven
他
http://hdl.handle.net/2324/4399994
出版情報：2018 European Control Conference (ECC). 4, pp.1523-1528, 2018-11-29. Institute of
Electrical and Electronics Engineers
バージョン：
権利関係：© 2018 IEEE. Personal use of this material is permitted. Permission from IEEE must
be obtained for all other uses, in any current or future media, including
reprinting/republishing this material for advertising or promotional purposes, creating new
collective works, for resale or redistribution to servers or lists, or reuse of any
copyrighted component of this work in other works.
Embedded nonlinear model predictive control for obstacle avoidance
using PANOC
Ajay Sathya, Pantelis Sopasakis, Ruben Van Parys, Andreas Themelis, Goele Pipeleers and Panagiotis Patrinos
Abstract— We employ the proximal averaged Newton-type
method for optimal control (PANOC) to solve obstacle avoid-
ance problems in real time. We introduce a novel modeling
framework for obstacle avoidance which allows us to easily
account for generic, possibly nonconvex, obstacles involving
polytopes, ellipsoids, semialgebraic sets and generic sets de-
scribed by a set of nonlinear inequalities. PANOC is particularly
well-suited for embedded applications as it involves simple steps,
its implementation comes with a low memory footprint and its
fast convergence meets the tight runtime requirements of fast
dynamical systems one encounters in modern mechatronics and
robotics. The proposed obstacle avoidance scheme is tested on
a lab-scale autonomous vehicle.
Index Terms— Embedded optimization, Nonlinear model pre-
dictive control, Obstacle avoidance.
I. INTRODUCTION
A. Background and Contributions
Autonomous navigation in obstructed environments is a
key element in emerging applications such as driverless
cars, fleets of automated vehicles in warehouses and aerial
robots performing search-and-rescue expeditions. Well-known
approaches for finding collision-free motion trajectories are
graph-search methods [1], virtual potential field methods [2]
or methods using the concept of velocity obstacles [3].
Recent motion planning research focuses on optimization-
based strategies. Here, an optimal motion trajectory is sought
while collision-avoidance requirements are imposed as con-
straints thereon. There is a broad range of such formulations.
The most straightforward approach constrains the Euclidean
distance between the vehicle and obstacle centers [4]. This,
however, only allows to separate spheres and ellipsoids as
the computation of distances from arbitrary sets is generally
not an easy operation.
Other approaches demand the existence of a hyperplane
between the vehicle and obstacle at each time instant [5],
[6]. This allows the separation of general convex sets. Some
methods formulate the collision avoidance requirement by
mixed-integer constraints [7]. These types of problems are,
however, cumbersome to solve in real-time. For some specific
problem types it is possible to transform the system dynamics
1
A. Sathya, R. Van Parys and G. Pipeleers are with the Dept. of Mechanical
Engineering, MECO research group, KU Leuven, 3001 Leuven, Belgium.
2
P. Sopasakis, A. Themelis and P. Patrinos are with the Dept. of Electrical
Engineering (ESAT-STADIUS), KU Leuven, 3001 Leuven, Belgium.
The work of the these authors was supported by the Research Founda-
tion Flanders (FWO) PhD fellowship 1196818N; FWO research projects
G086518N and G086318N; KU Leuven internal funding StG/15/043; Fonds
de la Recherche Scientifique – FNRS and the Fonds Wetenschappelijk
Onderzoek – Vlaanderen under EOS Project no 30468160 (SeLMA). R. Van
Parys is a PhD fellow of the Research Foundation Flanders (FWO-Flanders).
such that collision-avoidance translates into simple box
constraints on the states [8], [9].
Model predictive control (MPC) is a powerful control
strategy where control actions are computed by optimizing
a cost function which is chosen in order to achieve a
control task. Constraints on states, inputs and outputs can
be seamlessly incorporated into such a framework. When
the system dynamics is linear, the constraints are affine and
the cost functions are quadratic, the associated optimization
problem is a quadratic program. There exists a mature
machinery of convex optimization algorithms [10], [11] which
are fast, robust and possess global convergence guarantees
which can be used to solve these problems.
Nevertheless, the dynamics of most systems of interest are
better modeled by nonlinear equations and constraints are
often nonconvex. This situation is very common in obstacle
avoidance involving nonconvex optimization problems. These
are commonly solved using sequential quadratic program-
ming (SQP) [10] and interior point (IP) methods [12] which
are not well-suited for embedded applications with tight
runtime requirements. Nonlinear MPC is often performed
using the real-time iteration scheme proposed in [13] which
trades speed for accuracy and is accompanied by global
convergence guarantees under certain assumptions.
For an algorithm to be suitable for an embedded im-
plementation, it needs to involve only simple steps. This
deems methods of the forward-backward-splitting (FBS)
type [14], such as the proximal gradient method [15, Sec. 2.3],
appealing candidates. FBS-type algorithms can be used
to solve nonlinear optimal control problems with simple
input constraints via first eliminating the state sequence and
expressing the cost as a function of the sequence of inputs
alone — the so-called single shooting formulation. However,
despite its simplicity, FBS, like all first-order methods, can
exhibit slow convergence. Its convergence rate is at best Q-
linear with a Q-factor close to one for ill-conditioned problems
such as most nonlinear MPC problems.
In this work we propose a new modeling framework
for generic constraints which can accommodate general
nonconvex sets. The proposed methodology assumes that the
obstacles are described by a set of nonlinear inequalities and
does not require the computation of projections or distances
to them. Then, the obstacle avoidance constraints are written
as a nonlinear equality constraint involving a smooth function
which, in turn, is relaxed using a penalty function.
The resulting problems are solved using PANOC, a
proximal averaged Newton-type method for optimal control,
which was recently proposed in [16]. Gradients of the
cost function can be efficiently computed using automatic
differentiation toolboxes such as CasADi. The algorithm is
simple to implement, yet robust, since it combines projected
gradient iterations with quasi-Newtonian directions to achieve
fast convergence.
The proposed framework is tested on a number of sim-
ulation scenarios where we show that it is possible to
avoid obstacles of complex shape described by nonlinear
inequalities. PANOC is compared with SQP and IP methods
and is found to be significantly faster. Furthermore, we present
experimental results on a lab-scale robotic platform which
runs a C implementation of PANOC.
B. Notation
Let
IN
be the set of nonnegative integers,
IN
[k
1
,k
2
]
be the
set of integers in the interval
[k
1
, k
2
]
and
IR = IR∪{+∞}
be
the set of extended real numbers. For a matrix
A ∈ IR
m×n
,
we denote its transpose by
A
>
. For
x ∈ IR
, we define the
operator
[x]
+
= max{x, 0}
. For a nonempty closed convex
set
U ⊆ IR
n
, the projection onto
U
is the operator
Π
U
(v) =
argmin
u∈U
ku − vk
. The distance from the set
U
is defined
as
dist
U
(v) = inf
u∈U
ku − vk
. The class of continuously
differentiable functions
f : IR
n
→ IR
is denoted by
C
1
.
The subset of
C
1
of functions with Lipschitz-continuous
gradient is denoted as
C
1,1
. We use the notation
C
1,1
L
for
C
1,1
functions with L-Lipschitz gradients.
II. NMPC FOR OBSTACLE AVOIDANCE
A. Problem statement
Kinematic equations lead to continuous-time nonlinear
dynamical systems of the form
˙x = f
c
(x, u, t)
where
x ∈
IR
n
x
is the system state, typically a vector comprising of
position, velocity and orientation data, and
u ∈ IR
n
u
is
the control signal. We assume that the position coordinates
z ∈ IR
n
d
are part of the state vector. The continuous-time
dynamics can be discretized (for instance, using an explicit
Runge-Kutta method) leading to a discrete-time dynamical
system of the form
x
k+1
= f
k
(x
k
, u
k
). (1)
As it is typically the case in practice, we assume that
f
k
:
IR
n
x
× IR
n
u
→ IR
n
x
are smooth mappings.
The objective of the navigation controller is to steer the
controlled vehicle from an initial state
x
0
to a target state
x
ref
, typically a position in space together with a desired
orientation. At the same time, the vehicle has to avoid certain,
possibly moving, obstacles which are described by open sets
O
kj
⊆ IR
n
d
, j ∈ IN
[1,q
k
]
, each described by
O
kj
= {z ∈ IR
n
d
: h
i
kj
(z) > 0, i ∈ IN
[1,m
kj
]
}. (2)
Sets
O
kj
need not be convex. Obstacle avoidance constraints
can be concisely written as
z
k
/∈ O
kj
, for j ∈ IN
[1,q
k
]
. (3)
Moreover, the vehicle is only allowed to move in a domain
which is described by the inequalities
g
k
(x
k
, u
k
) ≤ 0, (4)
where
g
k
: IR
n
x
× IR
n
u
→ IR
n
c
is a
C
2
mapping and
≤
is meant in the element-wise sense. Control actions
u
k
are
constrained in a closed compact set U
k
, that is
u
k
∈ U
k
, (5)
on which it is easy to project and hereafter shall be assumed
to be convex. Sets
U
k
often represent box constraints of the
form U
k
= {u ∈ IR
n
u
: u
min
≤ u ≤ u
max
}.
B. Nonlinear model predictive control
Nonlinear model predictive control problems arising in
obstacle avoidance can be written in the following form
minimize `
N
(x
N
) +
N−1
X
k=0
`
k
(x
k
, u
k
), (6a)
subject to x
0
= x, (6b)
x
k+1
= f
k
(x
k
, u
k
), k ∈ IN
[0,N−1]
, (6c)
u
k
∈ U
k
, k ∈ IN
[0,N−1]
, (6d)
z
k
/∈ O
kj
, j ∈ IN
[1,q
k
]
, k ∈ IN
[0,N]
(6e)
g
k
(x
k
, u
k
) ≤ 0, k ∈ IN
[0,N]
, (6f)
g
N
(x
N
) ≤ 0. (6g)
The stage costs
`
k
: IR
n
x
× IR
n
u
→ IR
for
k ∈ IN
[0,N−1]
in
(6a)
are
C
1,1
functions penalizing deviations of the state
from the reference (destination and orientation) and may
be taken to be quadratic functions of the form
`(x
k
, u
k
) =
(x
k
− x
ref
)
>
Q
k
(x
k
− x
ref
) + (u
k
− u
ref
)
>
R
k
(u
k
− u
ref
)
. The
terminal cost
`
N
: IR
n
x
→ IR
in
(6a)
is a
C
1,1
function such
as `
N
(x
N
) = (x
N
− x
ref
)
>
Q
N
(x
N
− x
ref
).
C. Reformulation of obstacle avoidance constraints
Consider an obstacle described as the intersection of a
finite number of strict nonlinear inequalities
O = {z ∈ IR
n
d
: h
i
(z) > 0, i ∈ IN
[1,m]
}, (7)
where
h
i
: IR
n
d
→ IR
are
C
1,1
functions. The constraint
z /∈ O — cf. (6e) — is satisfied if and only if
h
i
0
(z) ≤ 0, for some i
0
∈ IN
[1,m]
, (8)
or, equivalently,
h
i
0
(z)
2
+
= 0
. This constraint can then be
encoded as
ψ
O
(z) :=
1
2
m
Y
i=1
h
i
(z)
2
+
= 0. (9)
We have expressed the obstacle avoidance constraints as a
nonlinear equality constraint. We should remark that, unlike
approaches based on the distance-to-set function [17], function
ψ
O
in
(9)
is a
C
1
function of
z
. Indeed,
ψ
O
is differentiable
in
IR
n
d
\ O
(and equal to
0
), it is differentiable in
O
with
gradient
∇ψ
O
(z) =
m
P
i=1
h
i
(z)
Q
j6=i
(h
j
(z))
2
∇h
i
(z), if x ∈ O,
0, otherwise.
(10)
Note that
∇ψ
O
on the boundary of
O
vanishes, so it is
everywhere continuous. If, additionally,
O
is bounded,
ψ
O
is
C
1,1
.
The formulation of Eq.
(9)
can be used for obstacles
described by quadratic constraints of the form
O = {z ∈
IR
n
d
: 1 − (z − c)
>
E(z − c) > 0},
such as balls and
ellipsoids. Then, the associated equality constraint becomes
[1 − (z − c)
>
E(z − c)]
2
+
= 0.
Polyhedral obstacles of the
form
O = {z ∈ IR
n
d
: b
i
− a
>
i
z > 0, ∈ IN
[1,m]
},
with
b
i
∈ IR
and
a
i
∈ IR
n
d
, can also be accommodated by the
constraint
Q
m
i=1
[b
i
− a
>
i
z]
2
+
= 0.
Equation
(9)
can also be used to describe general nonconvex
constraints such as ones described by semi-algebraic sets
where
h
i
(z)
are polynomials as well as any other obstacle
which is available in the aforementioned representation.
Obstacle avoidance constraints
(9)
are equivalent to the
existence of t ∈ IR
m
so that
min{t
1
, . . . , t
m
} = 0, (11a)
h
i
(z) ≤ t
i
, for i ∈ IN
[1,m]
. (11b)
This observation reveals a link to vertical complementarity
constraints which have been studied extensively in the
literature [18], [19].
D. Relaxation of constraints
Due to the fact that modeling errors and disturbances may
lead to the violation of imposed constraints and infeasibility of
the MPC optimization problem, it is common practice in MPC
to replace state constraints by appropriate penalty functions
known as soft constraints. Quadratic penalty functions are
often used for this purposes revealing a clear link between
this approach and the quadratic penalty method in numerical
optimization [15, Sec. 4.2.1].
Equality constraints, such as the ones arising in the refor-
mulation of the obstacle avoidance constraints in Section II-
C, can be relaxed by means of soft constraints. Indeed,
constraints of the form
Φ
k
(z
k
) = 0
,
k ∈ IN
[1,N]
, where
Φ
k
: IR
n
d
→ IR
+
are
C
2
functions, can be relaxed by
introducing the penalty functions
˜
Φ
k
(z
k
) = η
k
Φ
k
(z
k
)
for
some weight factors η
k
> 0.
That said, constraints like
(9)
for a set of time-varying
obstacles
O
kj
= {z ∈ IR
n
d
: h
i
kj
(z) > 0, i ∈ IN
[1,m
kj
]
}
, with
j ∈ IN
[1,q
k
]
, can be relaxed by the appending the following
term in the original cost function
˜
h
k
(z
k
) =
q
k
X
j=1
η
kj
m
kj
Y
i=1
h
i
kj
(z)
2
+
, (12)
for some positive weight factors η
kj
> 0.
Note that the proposed approach for dealing with obstacle
avoidance constraints requires only a representation of the
obstacles in the generic form
(7)
and does not call for the
computation of distances to the obstacles as in distance-based
methods [4], [17], nor does it require the obstacles to be
convex sets.
Similarly, inequality constraints of the form
g
k
(x
k
, u
k
) ≤
0
and
g
N
(x
N
) ≤ 0
can be relaxed by introducing the
penalty functions
˜g
k
(x, u) = β
k
[g
k
(x, u)]
2
+
and
˜g
N
(x) =
β
N
[g
N
(x)]
2
+
for positive weights β
k
> 0, k ∈ IN
[0,N]
.
We may now relax the state constraints in
(6)
by defining
the modified stage cost and terminal cost functions
˜
`
k
(x, u) = `
k
(x, u) + ˜g
k
(x, u) +
˜
h
k
(z
k
),
˜
`
N
(x) = `
N
(x) + ˜g
N
(x),
leading to the following relaxed optimization problem without
state constraints
minimize
˜
`
N
(x
N
) +
N−1
X
k=0
˜
`
k
(x
k
, u
k
), (13a)
subject to x
0
= x, (13b)
x
k+1
= f
k
(x
k
, u
k
), k ∈ IN
[0,N−1]
, (13c)
u
k
∈ U
k
, k ∈ IN
[0,N−1]
, (13d)
E. NMPC problem formulation
In this section we cast the nonlinear MPC problem
(6)
as
minimize
u∈U
`(u), (14)
where the optimization is carried out over vectors
u =
(u
0
, . . . , u
N−1
) ∈ IR
n
, with
n = N u
u
and
U := U
0
× U
1
×
· · · × U
N−1
and
` : IR
n
→ IR
is a real-valued
C
1,1
L
`
function.
We introduce the following sequence of functions
F
k
:
IR
n
→ IR
n
x
for k ∈ IN
[0,N−1]
F
0
(u) = x, (15a)
F
k+1
(u) = f
k
(F
k
(u), u
k
). (15b)
Define the smooth function ` : IR
n
→ IR
`(u) :=
˜
`
N
(F
N
(u)) +
N−1
X
k=0
˜
`
k
(F
k
(u), u
k
). (16)
The gradient of function
`
in
(16)
can be computed by means
of the reverse mode of automatic differentiation (also known
as adjoint method or backpropagation) as shown in Alg. 1 [20].
Algorithm 1 Automatic differentiation for ` in (16)
Input: x
0
∈ IR
n
x
, u ∈ IR
n
.
Output: `(u), ∇`(u)
1: `(u) ← 0
2: for k = 0, . . . , N − 1 do
3: x
k+1
← f
k
(x
k
, u
k
), `(u) ← `(u) +
˜
`
k
(x
k
, u
k
)
4: `(u) ← `(u) +
˜
`
N
(x
N
), p
N
← ∇
˜
`
N
(x
N
)
5: for k = N − 1, . . . , 0 do
6: p
k
← ∇
x
k
f
k
(x
k
, u
k
)p
k+1
+ ∇
x
k
˜
`
k
(x
k
, u
k
)
7: ∇
u
k
`
k
(u) ← ∇
u
k
f
k
(x
k
, u
k
) + ∇
u
k
˜
`
k
(x
k
, u
k
)
Problem
(14)
is in a form that allows the application of
the projected gradient iteration
u
ν+1
= T
γ
(u
ν
) := Π
U
(u
ν
− γ∇`(u
ν
)), (17)
with
γ > 0
. In particular, if
` ∈ C
1,1
L
`
and
γ <
2
/L
`
, then all
accumulation points of
(17)
,
u
?
, are fixed points of
T
γ
called
γ-critical points, that is [10, Prop. 2.3.2]
u
?
= T
γ
(u
?
). (18)
![Fig. 1. Construction of the FBE with γ = 0.15 (red line) for the function `(u) = sin(2u) over the set U = [0, 2] (thick blue line). The dashed line shows the approximation of ` at a point u by the quadratic model Q`γ(v;u). The value of FBE at u is then given by ϕγ(u) = infv∈U Q`γ(v;u). Note that the local minima of ` over U are exactly the local minima of ϕγ .](/figures/fig-1-construction-of-the-fbe-with-g-0-15-red-line-for-the-2asjvj9l.webp)
