Delft University of Technology

Model predictive contouring control for collision avoidance in unstructured dynamic

environments

Brito, Bruno; Floor, Boaz; Ferranti, Laura; Alonso-Mora, Javier

DOI
10.1109/LRA.2019.2929976
Publication date
2019
Document Version
Final published version
Published in
IEEE Robotics and Automation Letters

Citation (APA)
Brito, B., Floor, B., Ferranti, L., & Alonso-Mora, J. (2019). Model predictive contouring control for collision
avoidance in unstructured dynamic environments. IEEE Robotics and Automation Letters, 4(4), 4459-4466.
https://doi.org/10.1109/LRA.2019.2929976

Important note
To cite this publication, please use the final published version (if applicable).
Please check the document version above.

Copyright
Other than for strictly personal use, it is not permitted to download, forward or distribute the text or part of it, without the consent
of the author(s) and/or copyright holder(s), unless the work is under an open content license such as Creative Commons.
Takedown policy
Please contact us and provide details if you believe this document breaches copyrights.
We will remove access to the work immediately and investigate your claim.

This work is downloaded from Delft University of Technology.

For technical reasons the number of authors shown on this cover page is limited to a maximum of 10.
Green Open Access added to TU Delft Institutional Repository

'You share, we take care!' - Taverne project

https://www.openaccess.nl/en/you-share-we-take-care

Otherwise as indicated in the copyright section: the publisher

is the copyright holder of this work and the author uses the
Dutch legislation to make this work public.
IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019 4459

Model Predictive Contouring Control for Collision

Avoidance in Unstructured Dynamic Environments
Bruno Brito , Boaz Floor , Laura Ferranti , and Javier Alonso-Mora

Abstract—This letter presents a method for local motion plan-

ning in unstructured environments with static and moving ob-
stacles, such as humans. Given a reference path and speed, our
optimization-based receding-horizon approach computes a local
trajectory that minimizes the tracking error while avoiding obsta-
cles. We build on nonlinear model-predictive contouring control
(MPCC) and extend it to incorporate a static map by computing,
online, a set of convex regions in free space. We model moving
obstacles as ellipsoids and provide a correct bound to approximate
the collision region, given by the Minkowsky sum of an ellipse and
a circle. Our framework is agnostic to the robot model. We present
experimental results with a mobile robot navigating in indoor
environments populated with humans. Our method is executed
fully onboard without the need of external support and can be Fig. 1. The faculty corridor was the scenario used to evaluate the capabilities
applied to other robot morphologies such as autonomous cars. of our method to avoid dynamic obstacles.
Index Terms—Collision avoidance, motion and path planning.

Contouring Control (LMPCC) approach, suitable for real-time

I. INTRODUCTION collision-free navigation of AGVs in complex environments
PPLICATIONS where autonomous ground vehicles with several agents. Our design is fully implemented on board
A (AGVs) closely navigate with humans in complex en-
vironments require the AGV to safely avoid static and mov-
of the robot including localization and environment perception,
i.e., detection of static obstacles and pedestrians. Our design
ing obstacles while making progress towards its goal. Motion runs in real time thanks to its lightweight implementation. The
planning and control for AGVs are typically addressed as two method relies on an open-source solver [3] and will be released
independent problems [1]. In particular, the motion planner with this letter as a ROS module. Our method can be adapted to
generates a collision-free path and the motion controller tracks other robot morphologies, such as cars.
such a path by directly commanding the AGV’s actuators.
Our method combines both motion planning and control in A. Related Work
one module, relying on constrained optimisation techniques, Collision avoidance in static and dynamic environments can
to generate kinematically feasible local trajectories with fast be achieved via reactive methods, such as time-varying artificial
replanning cycles. More specifically, we rely on a Model Predic- potential fields [4], the dynamic window [5], social forces [6]
tive Controller (MPC) to compute an optimal control command and velocity obstacles [7]. Although these approaches work well
for the controlled system, which directly incorporates the pre- in low speed scenarios, or scenarios of low complexity, they pro-
dicted intentions of dynamic obstacles. Consequently, it reacts duce highly reactive behaviours. More complex and predictive
in advance to smoothly avoid moving obstacles. We propose behaviour can be achieved by employing a motion planner. Our
a practical reformulation of the Model Predictive Contouring method relies on model predictive control (MPC) [8], [9] to
Control (MPCC) approach [2], namely, a Local Model Predictive obtain smooth collision-free trajectories that optimize a desired
performance index, incorporate the physical constraints of the
Manuscript received February 24, 2019; accepted June 26, 2019. Date of robot and the predicted behavior of the obstacles.
publication July 22, 2019; date of current version October 24, 2019. This Due to the complexity of the motion planning problem, path
letter was recommended for publication by Associate Editor J.-M. Lien and
Editor N. Amato upon evaluation of the reviewers’ comments. This work was planning and path following were usually considered as two sep-
supported by the Amsterdam Institute for Advanced Metropolitan Solutions and arate problems. Many applications of MPC for path-following
the Netherlands Organisation for Scientific Research (NWO) domain Applied control are found in the literature, e.g., [10], [11]. These methods
Sciences. (Corresponding author: Bruno Brito.)
The authors are with the Department of Cognitive Robotics, Delft University assume the availability of a collision-free smooth path to follow.
of Technology, 2628 CD Delft, The Netherlands (e-mail: brunoffbrito0@gmail. In contrast, we use MPC for local motion planning and control
com; [email protected]; [email protected]; [email protected]). in the presence of static and dynamic obstacles.
This letter has supplementary downloadable material available at http://
ieeexplore.ieee.org, provided by the authors. Several approaches exist for integrated path following and
Digital Object Identifier 10.1109/LRA.2019.2929976 control for dynamic environments. These include: employing a
2377-3766 © 2019 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
4460 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

set of motion primitives and optimizing the control commands a socially-aware motion planner [23]. Finally, to illustrate the
to execute them [12] and offline computation of tracking error generality of the proposed MPCC framework, we present results
bounds via reachability analysis that ensures a safety region with a simulated car.
around the robot for online planning [13], [14]. These ap-
proaches, however, do not allow to incorporate the predicted II. PRELIMINARIES
intentions of the dynamic obstacles and consequently, can lead
A. Robot Description
to reactive behaviors. To overcome the latter issue, [15] proposed
the model predictive contouring control (MPCC) that allows Let B denote an AGV on the plane W = R2 . The AGV
to explicitly penalize the deviation from the path (in terms of dynamics are described by the discrete-time nonlinear system
contouring and lateral errors) and include additional constraints.
z(t + 1) = f (z(t), u(t)), (1)
Later, [16] designed a MPC for path following within a stable
handling envelope and an environmental envelope. While [16] is where z(t) and u(t) are the state and the input of the robot,
tailored to automotive applications without dynamic obstacles, respectively, at time t ≥ 0.1 For the case of our mobile robot we
MPCC has been employed to handle static and dynamic obsta- consider the state to be equal to the configuration, that is, z(t) ∈
cles in structured driving scenarios [2]. There, static collision C = R2 × S. For the case of a car (Section IV-E), the state space
constraints were formulated as limits on the reference path includes the speed of the car. The area occupied by the robot at
and thus limited to on-the-road driving scenarios. The previous state z is denoted by B(z).which is approximated by a union
approaches do not account for the interaction effects between of nc circles, i.e., B(z) ⊆ c∈{1,...,nc } Bc (z) ⊂ W. The center
the agents and may fail in crowded scenarios, a problem known of each circle, in the inertial frame, is given by p + RB W
(z)pBc .
as the freezing robot problem (FRP) [17]. Interaction Gaussian Where p is the position of the robot (extracted from z), RB W
(z)
Processes (IGP) [18] can be used to model each individual’s path. is the rotation matrix given by the orientation of the robot, and
The interactions are modeled with a nonlinear potential function. pBc is the center of circle c expressed in the body frame.
The resulting distribution, however, is intractable, and sampling
processes are required to approximate a solution, which requires B. Static Obstacles
high computational power and is only real-time for a limited
number of agents. Learning-based approaches address this issue The static obstacle environment is captured in an occupancy
by learning the collision-avoidance strategy directly from offline grid map, where the area occupied by the static obstacles is
simulation data [19], or the complex interaction model from raw denoted by Ostatic ⊂ W. In our experiments we consider both
sensor readings [20]. Yet, both methods learn a reactive colli- a global map, which is built a priori and used primarily for
sion avoidance policy and do not account for the kinodynamic localization, and a local map from the current sensor readings.
constraints of the robot. Therefore, the static map is continuously updated locally. Dy-
Without explicitly modeling interaction, we propose a method namic obstacles, such as people, that are recognized and tracked
for local motion planning, which is real-time, incorporates the by the robot are removed from the static map and considered as
robot constraints and optimizes over a prediction horizon. Our moving obstacles.
approach relies on MPCC and extends it to mobile robots operat-
ing in unstructured environments. Similar to [21], among others, C. Dynamic Obstacles
we employ polyhedral approximations of the free space, which Each moving obstacle i is represented by an ellipse of area
can provide larger convex regions than safety bubbles [22]. Ai ⊂ W, defined by its semi-major axis ai , its semi-minor axes
bi , and a rotation matrix Ri (ψ). We consider a set of moving
B. Contribution obstacles i ∈ I := {1, . . . , n}, where n can vary over time. The

We build on [2], with the following contributions to make the

area occupied by all moving obstacles at time instant t is given
by Otdyn = i∈{1,...,n} Ai (z i (t)), where z i (t) denotes the state
design applicable to mobile robots navigating in unstructured of moving obstacle i at time t. In this work, and without a
environments with humans: loss of generality, we assume a constant velocity model with
r A static obstacle avoidance strategy that explicitly con-
Gaussian noise ωo (t) ∼ (0, Qo (t)) in acceleration, that is,
strains the robot’s positions along the prediction horizon p̈i (t) = ωi (t), where pi (t) is the position of obstacle i at time t.
to a polyhedral approximation of the collision-free area Given the measured position data of each obstacle, we estimate
around the robot. their future positions and uncertainties with a linear Kalman
r A closed-form bound to conservatively approximate colli-
filter [24].
sion avoidance constraints that arise from ellipsoidal mov-
ing obstacles. D. Global Reference Path
r A fully integrated MPCC approach that runs in real-time
on-board of the robot and with on-board perception. Consider that a reference path is available. In its simplest
We present experimental results with a mobile robot navigat- form, the reference path can be a straight line to the goal
ing in indoor environments among static and moving obstacles
and compare them with three state-of-art planners, namely the 1 In the remainder of the letter we omit the time dependency when it is clear
dynamic window [5], a classical MPC for tracking [11] and from the context.
BRITO et al.: MODEL PREDICTIVE CONTOURING CONTROL FOR COLLISION AVOIDANCE IN UNSTRUCTURED DYNAMIC ENVIRONMENTS 4461

position or a straight line in the direction of preferred motion.

But it could also be given by a global planner. We consider
a global reference path P consisting of a sequence of path
segments connecting M way-points prm = [xpm , ym p
] ∈ W with
m ∈ M := {1, . . . , M }. For smoothness, we consider that each
path segment ςm (θ) is defined by a cubic polynomial. We denote
by θ a variable that (approximately) represents the traveled
distance along the reference path, and which is described in
more detail in Section III-C3. We do not require the reference to
be collision free, therefore, the robot may have to deviate from
it to avoid collisions. Fig. 2. Representation of the convex free space (orange squares) around
each prediction step on the prediction horizon (purple line) with respect to the
inflated static environment and the collision space of the dynamic obstacle (green
E. Problem Formulation ellipses) with respect to the vehicle representing discs (blue).
The objective is to generate, for the robot, a collision free
motion for N time-steps in the future, while minimizing a
cost function J that includes a penalty for deviations from the [p∗1:N |t−1 , q N ], where q N is a extrapolation of the last two points,
reference path. This is formulated in the optimization problem that is, q N = 2p∗N |t−1 − p∗N −1|t−1 . Then, for each point q k
N −1 (k = 1, . . . , N ) we compute a convex region in free 4 space, given
 stat,l
J∗ = min J(z k , uk , θk ) + J(z N , θN ) by a set of four linear constraints cstat k (p k ) = l=1 kc (pk ).
z 0:N ,u0:N −1 ,θ0:N −1
k=0 This region separates q k from the closest obstacles. In our im-
plementation we compute a rectangular region aligned with the
s.t. z k+1 = f (z k , uk ), θk+1 = θk + vk τ, (2a) orientation of the trajectory at q k , where each linear constraint
  is obtained by a search routine and reduced by the radius of the
B(z k ) ∩ Ostatic ∪ Okdyn = ∅, (2b)
robot circles rdisc . Fig. 2 shows the collision-free regions along
uk ∈ U , z k ∈ Z, z 0 , θ0 given. (2c) the prediction horizon defined as yellow boxes. At prediction
where vk is the forward velocity of the robot (for the mobile robot step k for a robot with state z the resulting constraint for disc j
it is part of the input and for the car it is part of the state), τ is the and polygon side l is
time-step and Z and U are the set of admissible states and inputs,  
cstat,l,j (z) = hl − nl · p − RB W
(z)pB j > 0, (3)
respectively. z 1:N and u0:N −1 are the set of states and control
inputs, respectively, over the prediction horizon Thorizon , which where hl and nl define each side of the polygons.
is divided into N prediction steps. θk denotes the predicted
progress along the reference path at time-step k. By solving
B. Dynamic Collision Avoidance
the optimization problem, we obtain a locally optimal sequence
of commands [u∗t ]t=Nt=0
−1
to guide the robot along the reference Recall that each moving obstacle i is represented by its
path while avoiding collisions with static and moving obstacles. position pi (t) and an ellipse of semi-axis ai and bi and a rotation
matrix Ri (ψ). For each obstacle i ∈ {1, . . . , n}, and prediction
III. METHOD step k, we impose that each circle j of the robot does not intersect
with the elliptical area occupied by the obstacle. Omitting i for
The proposed method consists of the following steps, which simplicity, the inequality constraint on each disc of the robot
are executed in every planning loop. with respect to the obstacles is
1) Search for a collision-free region in the updated static map
 j T 1   j
centered on the robot and constrain the control problem Δx 2 0 Δxk
k α
such that the robot remains inside (Section III-A). cobst,j
k (z k ) = j
R(ψ)T 1 R(ψ) > 1,
2) Predict the future positions of the dynamic obstacles and Δyk 0 β2 Δykj
use the corrected bound to ensure dynamic collision avoid- (4)
ance (Section III-B).
where the distance between disc j and the obstacle is separated
3) Solve a modified MPCC formulation applicable to mobile
into its Δxj and Δy j components (Fig. 2). The parameters α
robots (Section III-C).
and β are the semi-axes of an enlarged ellipse that includes the
union of the original ellipse and the circle.
A. Static Collision Avoidance While previous approaches approximated the Minkowski sum
Given the static map of the environment, we compute a set of of the ellipse with the circle as an ellipse of semi-major α = a +
convex four-sided polygons in free space. This representation rdisc and semi-minor axis β = b + rdisc [2], this assumption is not
can provide larger collision-free areas compared with other correct and collisions can still occur [25]. We now describe how
approximations such as circles. to compute the values for α and β such that collision avoidance
To obtain the set of convex regions at time t, we first shift is guaranteed, represented by the larger in light red ellipsoid in
the optimal trajectory computed at time t − 1, namely, q 0:N = Fig. 3.
4462 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

at. 3 This value of semi-axis α = a + δ and β = b + δ guarantee

that the constraint ellipsoid entirely bounds the collision space.

C. Model Predictive Contouring Control

MPCC is a formulation specially tailored to path-following
problems. This section presents how to modify the baseline
method [2] and make it applicable to mobile robots navigating
in unstructured environments with on-board perception.
1) Progress on Reference Path: Eq. (2) approximates the
evolution of the path parameter by the travelled distance of the
robot. In each planning stage we initialize θ0 . We find the closest
Fig. 3. Safety boundary computation. The ellipsoid with semi-major axis and path segment, denoted by m, and compute the value of θ0 via a
semi-minor axis, a and b respectively, is represented in green, the ellipsoid with line search in the neighborhood of the previously predicted path
axis enlarged by rdisc is represented in gray, the Minkowsky sum is represented
in black and the ellipsoid enlarged by δ is represented in red. parameter.
2) Selecting the Number of Path Segments: As detailed in
Section II-D, the global reference is composed of M path seg-
ments. To lower the computational load, only η ≤ M path seg-
Consider two ellipsoids E1 = Diag( a12 b12 ) and E2 =
1 1 ments are used to generate the local reference that is incorporated
Diag( (a+δ) 2 , (b+δ)2 ). E1 is an ellipsoid with a and b as semi-
into the optimization problem. The number of path segments η
major and semi-minor axes, respectively. E2 represents the cannot be arbitrarily small, and there is a minimum number of
ellipsoid E1 enlarged by δ in both axis. The goal is to find segments to be selected to ensure the robot follows the reference
the smallest ellipsoid that bounds the Minkowsky sum. This is path along a prediction horizon. The number of path segments η
equivalent to find the minimum value of δ such that the minimum in the local reference path is a function of the prediction horizon
distance between ellipsoid E1 and E2 is bigger than r2 , the radius length, the individual path segment lengths, and the speed of
of the circle bounding the robot. the robot at each time instance. We select a conservative η
Lemma 1 ([26]): Let X T A1 X = 1 and X T A2 X = 1 be two by considering the maximum longitudinal velocity vmax and
quadratics in Rn . Iff the matrix A1 − A2 is sign definite, then imposing that the covered distance is lower than a lower bound
the square of the distance between the quadratic X T A1 X = 1 of the travelled distance along the reference path, namely,
and the quadratic X T A2 X = 1 equals the minimal positive zero
of the polynomial. N
 −1 m+η
 m+η

τ vj ≤ τ N vmax ≤ ||pri+1 − pri || ≤ si ,
F (z) = Dλ (det λA1 + (z − λA2 ) − λ(z − λA1 A2 )) j=0 i=m+1 i=m+1
        
where D stands for the discriminant of the polynomial treated Traveled dist. Waypoints dist. Ref. path length
with respect to λ. (6)
Considering A1 = E1 , A2 = E2 and {δ, a, b, } ∈ R+ this where m is the index of the closest path segment to the robot, τ
ensures that E1 − E2 is sign definite. Hence, we can apply is the length of the discretization steps along the horizon, and si
Lemma 1 to determine the polynomial F (z) and its roots. For the length of each path segment.
the two ellipsoids E1 and E2 , the roots λ of F (z) are: 3) Maintaining Continuity Over the Local Reference Path:
⎧ ⎫ We concatenate the η reference path segments into a differen-
⎪ (2a(r + δ)3 + 2b(r + δ)3 + 4ab(r + δ)2 ⎪ tiable local reference path Lr , which will be tracked by the
⎪
⎪ , ⎪
⎪
⎪
⎪ (a2 + 2ab + 2ra + b2 + 2rb ⎪
⎪ LMPCC, as follows
⎪
⎪ ⎪
⎪
⎨ 2
⎬
λ∈ (r + δ) , . (5)
m+η

⎪
⎪ 2 ⎪
⎪ p̄r (θk ) = σi,+ (θk )σi,− (θk )ςi (θk ), (7)
⎪
⎪ 4a 2
+ 4a(r + δ) + (r + δ) , ⎪
⎪
⎪
⎪ ⎪
⎪ i=m
⎪
⎩ ⎪
2 2⎭ i
4b + 4b(r + δ) + (r + δ) where σi,− (θk ) = 1/(1 + e(θ− j=m si )/ ) and σi,+ (θk ) =
i−1
The first two roots have multiplicity two. The minimum distance 1/(1 + e(−θ+ j=m si )/ ) are two sigmoid activation functions
equation is the square root of the minimal positive zero of F (z). for each path segment and is a small design constant. This
Thus, the minimum enlargement factor is found by solving for representation ensures a continuous representation of the local
the value of δ that satisfies minj λ(j) = r2 . reference path needed to compute the solver gradients.
A closed form formula can be obtained by noting that the 4) Cost Function: For tracking of the reference path, a con-
minj λ(j) is achieved for the first root and solving this equation. tour and a lag error are defined, see Fig. 4 and combined in an
Due to its length, it is not presented in this letter but can be found

3 A Mathemathica notebook with the derivation of the bound and

2 Note we use r instead of rdisc in the reminder of the section to simplify the a Matlab script as example of its computation can be found in
notation. http://www.alonsomora.com/docs/19-debrito-boundcomputation.zip.
BRITO et al.: MODEL PREDICTIVE CONTOURING CONTROL FOR COLLISION AVOIDANCE IN UNSTRUCTURED DYNAMIC ENVIRONMENTS 4463

Algorithm 1: Local Model Predictive Contouring Control.

1: Given z init , z goal , Ostatic , O0dyn , and N
2: for t = 0, 1, 2, . . . do
3: z 0 = z init
4: Estimate θ0 according to Section III-C1
5: Select η according to Eq. (6)
6: Build p̄r (θk ), k = 1, . . . , N , according to Eq. (7)
7: Compute cstat,jk (pk ) along q 0:N
8: Get dynamic-obstacles predicted pose
(Section III-B)
Fig. 4. Approximated contour and lag errors on the path segment. 9: Solve the optimization problem of Eq. (11)
10: Apply u∗0
11: end for
error vector ek := [˜c (z k , θk ), ˜l (z k , θk )]T , with
 
sin φ(θk ) − cos φ(θk )  
ek = pk − p̄r (θk ) , (8) cost is J(z N , θN ) := Jtracking (z N , θN ) + Jrepulsive (z N ).
− cos φ(θk ) − sin φ(θk ) Eq. (11c) and Eq. (11d) are defined by Eq. (3) and Eq. (4),
where φ(θk ) = arctan(∂y r (θk )/∂xr (θk ) is the direction of the respectively. Algorithm 1 summarizes our method. Note that at
path. Consequently, the LMPCC tracking cost is each control iteration, we solve (using [3]) Problem (11) until
either a Karush-Kuhn-Tucker condition [27] or the maximum
Jtracking (z k , θk ) = eTk Q ek , (9) number of iterations (itermax ) is satisfied (line 1). The
where Q is a design weight. selection of itermax is empirically based on the maximum
The solution which minimizes the quadratic tracking cost number of iterations allowed within the sampling time of our
defined in Eq. (9) drives the robot towards the reference path. system to guarantee real-time performance.
To make progress along the path we introduce a cost term that
penalizes the deviation of the robot velocity vk from a reference IV. RESULTS
velocity vref , i.e., Jspeed (z k , uk ) = Qv (vref − vk )2 with Qv a de- This section presents experimental and simulation results
sign weight. This reference velocity is a design parameter given for three scenarios with a mobile robot. We evaluate different
by a higher-level planner and can vary across path reference settings for the parameters in our planner (Section IV-B), as well
segments. as compare its performance in static (Section IV-C) and dynamic
To increase the clearance between the robot and moving (Section IV-D) environments against state of art motion planners
obstacles, we introduce an additional cost term similar to a [5], [11], [23]. A video demonstrating the results accompanies
potential function, this letter.
n  
1
Jrepulsive (z k ) = QR , (10) A. Experimental Setup
i=1
(Δxk )2 + (Δyk )2 + γ
1) Hardware Setup: Our experimental platform is a fully
where QR is a design weight, and Δxk , Δyk represent the com-
autonomous Clearpath Jackal ground robot, for which we im-
ponents of the distance from the robot to the dynamic obstacles.
plemented on-board all the modules used for localization, per-
A small value γ ≥ 0 is introduced for numerical stability. Eq.
ception, motion planning, and control. Our platform is equipped
(10) adds clearance with respect to obstacles and renders the
with an Intel i5 [email protected], which is used to run the lo-
method more robust to localization uncertainties.
calization and motion planning modules, a Lidar Velodyne for
Additionally, we penalize the inputs with Jinput (z k , θk ) =
perception, and an Intel i7 NUC mini PC to run the pedestrian
uTk Qu uk , where Qu is a design weight.
tracker.
The LMPCC control problem is then given by a receding
2) Software Setup: To build the global reference path we
horizon nonconvex optimization, formally,
first define a series of waypoints and we construct a smooth
N
 −1 global path by connecting the waypoints with a clothoid. We
∗
J = min J(zk , uk , θk )+J(z N , θN ) (11a) then sample intermediate waypoints from this global path and
z 0:N ,u0:N −1 ,θ0:N
k=0 connect them with 3rd order polynomials, which are then used
s.t. : (2a), (2c), (11b) to generate the local reference path.
The robot localizes with respect to a map of the environment,
cstat,l,j
k (z k ) > 0, ∀j ∈ {1, . . . , nc }, l ∈ {1, . . . , 4} (11c) which is created before the experiments. For static collision
avoidance the robot utilizes a map that is updated online with
cobst,j
k (z k ) > 1, ∀j ∈ {1, . . . , nc }, ∀ obst (11d)
data from its sensors and we employ a set of rectangles to
where the stage cost is J(zk , uk , θk ) := Jtracking (z k , θk ) + model the free space. Our search routine expands the sides of a
Jspeed (z k , uk ) + Jrepulsive (z k ) + Jinput (uk ) and the terminal vehicle-aligned rectangle simultaneously in the occupancy grid
4464 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

Fig. 6. Computation time required to solve the LMPCC problem online for
different velocity references and horizon lengths. The central mark indicates
the median. The bottom and top edges of the box indicate the 25th and 75th
percentiles, respectively. The red crosses represent the outliers.

TABLE I
Fig. 5. Experiment for evaluation of performance. CLEARANCE BETWEEN THE ROBOT AND THE DYNAMIC OBSTACLES FOR
DIFFERENT VELOCITY REFERENCES AND HORIZON LENGTHS

environment with steps of Δsearch = 0.05 m, until either an occu-

pied cell is found or the maximum search distance Δsearchmax = 2 m
is reached. Once an expanding rectangle side is fixed as a result of
an occupied cell, the rest of the rectangle sides are still expanded
to search for the largest possible area. This computation runs in
parallel to the LMPCC solver, in a different thread, and with
the latest available information. Our experiments employ the
open-source SPENCER Pedestrian tracker and 2d laser data [28]
for detection and tracking of dynamic obstacles. If a pedestrian
is detected, it is removed from the static map and treated as
a moving ellipse. Our simulations use the open-source ROS
implementation of the Social Forces model [29] for pedestrian
simulation.
The LMPCC problem of Eq. (11) is nonconvex. Our planner
solves this problem online in real time using ACADO [3] and
its C-code generation tool. We use a continuous-time kinematic Fig. 7. Static collision avoidance scenario. The red crosses depict the path
unicycle motion model [30] to describe the robot’s kinemat- to follow (waypoints) and the colored points are the laser scan data. The blue,
ics. The model is then discretized directly in ACADO using green, and magenta lines represent the trajectory obtained with the LMPCC,the
MPC tracking controller, and the Dynamic Window, respectively.
a multiple-shooting method combined with a Gauss-Legendre
integrator of order 4, no Hessian approximations, and a sampling
time of 50 ms. We select qpOASES [31] to solve the resulting
QP problem and set a KKT tolerance of 10−4 and a maximum position of the obstacles and robot. Fig. 6 shows the computation
of 10 iterations. If no feasible solution is found within the time to solve the optimization problem. For vref ∈ {1, 1.25, 1.5}
maximum number of iterations, then the robot decelerates. The m/s and THorizon ∈ {1, 3} s the computation times are under
planner computes a new solution in the next cycle. Based on our 50 ms, which is lower than the cycle-time defined for the planner.
experience, this allowed to recover the feasibility of the planner But, for THorizon = 5 s the 99th percentile is above the 50 ms,
quickly. Our motion planner is implemented in C++/ROS and not respecting the real-time constraint. The cases in which
will be released open source. the planner exceeds the sampling time of the system are the
situations in which the pedestrians suddenly step in front of the
robot or change their direction of motion, requiring the solver
B. Parameter Evaluation more iterations to find a feasible solution. If not all the constraints
To evaluate the performance of the planner we per- can be satisfied, our problem becomes infeasible, and no solution
formed several experiments at different reference speeds, vref ∈ is found. In this case, we reduce the robot velocity, allowing
{1 m/s, 1.25 m/s, 1.5 m/s}, and prediction-horizon lengths, the solver to recover the feasibility after few iterations. Table I
THorizon ∈ {1 s, 3 s, 5 s}. The robot follows a Fig. 8 path (red summarizes the behavior of the planner in terms of clearance,
line in Fig. 5(a)) while avoiding two pedestrians (green ellipses) that is, the distance from the border of the circle of the robot to
and staying within the collision-free area (yellow rectangles). the border of the ellipse defining the obstacles. It demonstrates
We use one circle to represent the planned position of the that a short horizon leads to lower safety distances and more
robot (light blue circles). Each pedestrian is bounded by an importantly, that our method was able to keep a safe clearance
ellipse of semi-axis 0.3 m and 0.2 m. The predicted positions in most cases.
of the pedestrians are represented by a green line. We align the Based on these results, we selected a reference speed of
semi-minor axis to the pedestrian walking direction. For this 1.25 m/s and a horizon length of 3 s for the following
experiment we rely on a motion capture system to obtain the experiments.
BRITO et al.: MODEL PREDICTIVE CONTOURING CONTROL FOR COLLISION AVOIDANCE IN UNSTRUCTURED DYNAMIC ENVIRONMENTS 4465

TABLE II
STATISTIC RESULTS OF MINIMUM DISTANCE TO THE PEDESTRIANS (WHERE CLEARANCE IS DEFINED AS THE BORDER TO BORDER DISTANCE), TRAVELING
DISTANCE AND PERCENTAGE OF FAILURES OBTAINED FOR 100 RANDOM TEST CASES OF THE DYNAMIC COLLISION AVOIDANCE EXPERIMENT FOR n ∈ {2, 4, 6}
AGENTS. THE PEDESTRIANS FOLLOW THE SOCIAL FORCES MODEL [29]

C. Static Collision Avoidance

In this experiment we compare the proposed planner with two
baseline approaches:
r A MPC tracking controller [11]: We minimize the deviation
from positions on the reference path up to 1 m ahead of the
robot.
r The Dynamic Window (DW) [5]: We use the open-source
ROS stack implementation. The DW method receives the
next waypoint once the distance to the current waypoint is
less than 1 m.
For this and the following experiment, we fully rely on the
onboard localization and perception modules. A VLP16 Velo-
Fig. 8. Dynamic collision avoidance scenario. The red crosses represent the
dyne Lidar is used to build and update a local map centered in global path to follow (waypoints). The blue and magenta lines represent the
the robot for static collision avoidance. The prebuilt offline map trajectory executed by our LMPCC (top) and by the Dynamic Window (bottom),
is only used for localization. respectively. The trajectories of the two pedestrians are represented by the green
and magenta circles. In the lower case (dynamic window) the robot reacts late
In this experiment the mobile robot navigates along a corridor and the pedestrians must actively avoid it.
while tracking a global reference path (red waypoints in Fig. 7).
When it encounters automatic doors, the robot must wait for
them to open. When it encounters the obstacle located near
the third upper waypoint from the left, which was not in the To avoid the overlap of the static and dynamic collision con-
map, the robot must navigate around it. Fig. 7 shows the results straints, the detected pedestrians are removed from the updated
of this experiment. All the three approaches are able to follow map used for static avoidance and modeled as ellipses with
the waypoints and interact with the automatic door. When they a constant velocity estimate. The global path, as described in
encounter the second obstacle the classical MPC design fails to Section IV-A, consists of a straight line along the corridor. The
proceed towards the next waypoint. The DW and the LMPCC robot has to follow this path while avoiding collisions with
approaches are able to complete the task. Both methods showed several pedestrians moving in the same or opposite direction. In
similar performance (e.g., the traveled distance was 30.59 m this experiment, we do not evaluate the MPC tracking controller
for our LMPCC and 30.61 m for the DW). The time to the since it was unable to complete the previous experiment. Ag-
goal was relatively higher for the DW (50 s) compared to the gregated results in Table II show that the LMPCC outperforms
LMPCC (41 s). This difference is mainly due to the ability the other methods. It achieves a considerably lower failure
of the LMPCC to follow a reference velocity. In addition, we rate, smaller traveling distances, and maintains larger safety
tested DW and LMPCC in a scenario where half of the first door distances to the pedestrians. Only for the four pedestrians case,
does not open due to malfunction. In such scenario, the LMPCC the DW achieved larger mean clearance, but with larger standard
was able to traverse around the broken door while the DW gets deviation. By accounting for the predictions of the pedestrians,
into a deadlock state due to the narrow opening. A video of the our method can react faster and thus generate safer motion plans.
experiments accompanies the letter. Table II also shows that the number of failures grows with the
number of agents. Yet, our method can scale up to six pedestrians
with low collision probability and perform real-time. For larger
crowds our method would select the closer 6 pedestrians.
D. Dynamic Collision Avoidance 2) Experimental Results: We use the previous setup to com-
Our simulations compare our planner with an additional base- pare the LMPCC and DW methods on a real scenario with two
line: a socially-aware motion planner named Collision Avoid- pedestrians. We do not test the CADRL method because the
ance with Deep RL (CADRL) [23]. We employ the open-source current open-source implementation does not allow static colli-
ROS stack implementation. The CADRL method receives the sion avoidance for unconstrained scenarios such as the faculty
same waypoints as the DW method. corridor depicted in Fig. 8. Fig. 8 shows one representative run
1) Simulation Results: We compare the performance of the of our method (top) and the DW (bottom). We observe that the
MPCC planner with the DW and CADRL baselines in the pres- proposed method reacts in advance to avoid the pedestrians,
ence of pedestrians. Fig. 1 shows the setup of our experiment. resulting in a larger clearance distance. In contrast, the DW reacts
4466 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

late to avoid the pedestrian, which has to avoid the robot himself [7] J. Alonso-Mora, P. Beardsley, and R. Siegwart, “Cooperative collision
actively. Our proposed method was able to navigate safely in all avoidance for nonholonomic robots,” IEEE Trans. Robot., vol. 34, no. 2,
pp. 404–420, Apr. 2018.
of our experiments with static and two pedestrians. [8] J. M. Maciejowski, Predictive Control: With Constraints. London, U.K.:
Pearson, 2002.
E. Applicability to an Autonomous Car [9] F. Borrelli, A. Bemporad, and M. Morari, Predictive Control for Linear
and Hybrid Systems. Cambridge, U.K.: Cambridge Univ. Press, 2017.
To validate the applicability of our method to more complex [10] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, “MPC-based
approach to active steering for autonomous vehicle systems,” Int. J. Veh.
robot models, we have performed a simulation experiment with Auton. Syst., vol. 3, no. 2–4, pp. 265–291, 2005.
a kinematic bicycle model [32] of an autonomous car. [11] C. M. Kang, S. Lee, and C. C. Chung, “On-road path generation and
The planner commands the acceleration and front steering. control for waypoints tracking,” IEEE Intell. Transp. Syst. Mag., vol. 9,
no. 3, pp. 36–45, Fall 2017.
The car follows a global reference path while staying within [12] T. M. Howard, C. J. Green, and A. Kelly, “Receding horizon model-
the road boundaries (i.e., the obstacle-free region) and avoiding predictive control for mobile robot navigation of intricate paths,” in Field
moving obstacles (such as a simulated cyclist proceeding in the and Service Robotics, 2010, pp. 69–78.
[13] D. Fridovich-Keil, S. L. Herbert, J. F. Fisac, S. Deglurkar, and C. J.
direction of the car and a pedestrian crossing the road in front of Tomlin, “Planning, fast and slow: A framework for adaptive real-time
the car). The accompanying video shows the results where the safe trajectory planning,” in Proc. IEEE Int. Conf. Robot. Autom., 2018,
autonomous vehicle successfully avoids the moving obstacles, pp. 387–394.
[14] J. F. Fisac et al., “Probabilistically safe robot planning with confidence-
while staying within the road limits. We refer the reader to [33] based human predictions,” Robot.: Sci. Syst., 2018.
for more details and results. [15] D. Lam, C. Manzie, and M. Good, “Model predictive contouring control,”
in Proc. 49th IEEE Conf. Decis. Control, 2010, pp. 6137–6142.
[16] M. Brown, J. Funke, S. Erlien, and J. C. Gerdes, “Safe driving envelopes
V. CONCLUSIONS & FUTURE WORK for path tracking in autonomous vehicles,” Control Eng. Pract., vol. 61,
pp. 307–316, 2017.
This letter proposed a local planning approach based on [17] T. Kruse, A. K. Pandey, R. Alami, and A. Kirsch, “Human-aware robot
Model Predictive Contouring Control (MPCC) to safely navigate navigation: A survey,” Robot. Auton. Syst., vol. 61, pp. 1726–1743, 2013.
a mobile robot in dynamic, unstructured environments. Our [18] P. Trautman, “Sparse interacting Gaussian processes: Efficiency and op-
timality theorems of autonomous crowd navigation,” in Proc. IEEE 56th
local MPCC relies on an upper bound of the Minkowski sum Annu. Conf. Decis. Control, 2017, pp. 327–334.
of a circle and an ellipse to safely avoid dynamic obstacles [19] B. Lütjens, M. Everett, and J. P. How, “Safe reinforcement learning
and a set of convex regions in free space to avoid static obsta- with model uncertainty estimates,” in Proc. Int. Conf. Robot. Autom.,
2019, pp. 8662–8668.
cles. We compared our design with three baseline approaches [20] P. Long, T. Fanl, X. Liao, W. Liu, H. Zhang, and J. Pan, “Towards optimally
(classical MPC, Dynamic Window, and CADRL). The exper- decentralized multi-robot collision avoidance via deep reinforcement
imental results demonstrate that our method outperforms the learning,” in Proc. IEEE Int. Conf. Robot. Autom., 2018, pp. 6252–6259.
[21] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot formation control and
baselines in static and dynamic environments. Moreover, the object transport in dynamic environments via constrained optimization,”
light implementation of our design shows the scalability of our Int. J. Robot. Res., vol. 36, no. 9, pp. 1000–1021, Jul. 2017.
method up to six agents and allowed us to run all algorithms [22] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach
to smooth trajectories for motion planning with car-like robots,” in Proc.
on-board. Finally, we showed the applicability of our design to 54th IEEE Conf. Decis. Control, 2015, pp. 835–842.
more complex robots by testing the design in simulation using [23] M. Everett, Y. F. Chen, and J. P. How, “Motion planning among dy-
the model of an autonomous car. As future work, we intend to namic, decision-making agents with deep reinforcement learning,” in Proc.
IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 3052–3059.
expand our approach for crowded scenarios, by accounting for [24] G. Welch et al., “An introduction to the Kalman filter,” 2006.
the interaction effects between the robot and the other agents. [25] J. Chen, W. Zhan, and M. Tomizuka, “Constrained iterative LQR for on-
road autonomous driving motion planning,” in Proc. 20th Int. Conf. Intell.
Transp. Syst., 2017, pp. 1–7.
REFERENCES [26] A. Y. Uteshev and M. V. Yashina, “Metric problems for quadrics in
[1] W. Schwarting, J. Alonso-Mora, and D. Rus, “Planning and decision- multidimensional space,” J. Symbolic Comput., vol. 68, pp. 287–315, 2015.
making for autonomous vehicles,” Annu. Rev. Control, Robot., Auton. Syst., [27] H. W. Kuhn and A. W. Tucker, “Nonlinear programming,” in Proc. 2nd
vol. 1, no. 1, pp. 187–210, 2018. Berkeley Symp. Math. Statist. Probability, 1951, pp. 481–492.
[2] W. Schwarting, J. Alonso-Mora, L. Paull, S. Karaman, and D. Rus, “Safe [28] T. Linder, S. Breuers, B. Leibe, and K. O. Arras, “On multi-modal people
nonlinear trajectory generation for parallel autonomy with a dynamic vehi- tracking from mobile platforms in very crowded and dynamic environ-
cle model,” IEEE Trans. Intell. Transp. Syst., vol. 19, no. 9, pp. 2994–3008, ments,” in Proc. IEEE Int. Conf. Robot. Autom., 2016, pp. 5512–5519.
Sep. 2018. [29] D. Helbing and P. Molnar, “Social force model for pedestrian dynamics,”
[3] B. Houska, H. Ferreau, and M. Diehl, “ACADO toolkit—An open source Phys. Rev. E, vol. 51, no. 5, 1995, Art. no. 4282.
framework for automatic control and dynamic optimization,” Optimal [30] R. Siegwart and I. R. Nourbakhsh, Introduction to Autonomous Mobile
Control Appl. Methods, vol. 32, pp. 298–312, 2011. Robots, 2nd ed. Cambridge, MA, USA: MIT Press, 2011.
[4] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile [31] H. J. Ferreau, C. Kirches, A. Potschka, H. G. Bock, and M. Diehl, “qpoases:
robots,” in Proc. Auton. Robot Veh., 1986, pp. 396–404. A parametric active-set algorithm for quadratic programming,” Math.
[5] D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to Program. Comput., vol. 6, pp. 327–363, 2014.
collision avoidance,” IEEE Robot. Autom. Mag., vol. 4, no. 1, pp. 23–33, [32] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and
Mar. 1997. dynamic vehicle models for autonomous driving control design.” in Proc.
[6] G. Ferrer, A. Garrell, and A. Sanfeliu, “Robot companion: A social- IEEE Intell. Veh. Symp., 2015, pp. 1094–1099.
force based approach with human awareness-navigation in crowded en- [33] L. Ferranti et al., “SafeVRU: A research platform for the interaction of
vironments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2013, self-driving vehicles with vulnerable road users,” in Proc. IEEE Intell. Veh.
pp. 1688–1694. Symp., 2019.