Available online at www.sciencedirect.

com

ScienceDirect
IFAC PapersOnLine 53-2 (2020) 9590–9595
VFH+D:
VFH+D: An
An Improvement
Improvement on
on the
the VFH+
VFH+
VFH+D:
Algorithm
VFH+D: An
for
An Improvement
Dynamic on
Obstacle
Improvement on the VFH+
Avoidance
the VFH+
Algorithm
Algorithm for
for Dynamic
Dynamic Obstacle

Obstacle Avoidance
Avoidance
Algorithm and
for
and Local
Dynamic
Local Planning
Obstacle
Planning  Avoidance
and Local
and Local Planning
Planning  ∗
∗
Daniel Dı́az ∗∗ Leonardo Marı́n ∗∗
Daniel Dı́az ∗ Leonardo Marı́n ∗
Daniel
Daniel Dı́az Dı́az ∗
∗ Leonardo
Leonardo Marı́n Marı́n ∗
∗
∗
∗ Electrical
∗ Engineering School, University of Costa Rica,
Ciudad ∗ Electrical Engineering School, University of Costa Rica,
de la Investigación, 11501 SanUniversity
Pedro, San José, Costa Rica.
Ciudad ∗ Electrical
∗
Electrical Engineering
Engineering11501
de la Investigación, School,
School,
SanUniversity
Pedro, San of Costa
of José,
CostaCostaRica,
Rica, Rica.
Ciudad (e-mail:
de la {daniel.diazmolina,
Investigación, 11501 leonardo.marin}@ucr.ac.cr).
San
Ciudad (e-mail: {daniel.diazmolina,
de la Investigación, San Pedro, San José, Costa Rica.
Pedro, San
11501 leonardo.marin}@ucr.ac.cr). José, Costa Rica.
(e-mail: {daniel.diazmolina, leonardo.marin}@ucr.ac.cr).
(e-mail: {daniel.diazmolina, leonardo.marin}@ucr.ac.cr).
Abstract: This paper highlights some limitations of the VFH+ algorithm on the domain of
Abstract: This paper highlights some limitations of the VFH+ algorithm on the domain of
local obstacle
Abstract: Thisavoidance. An enhanced
paper highlights some algorithm
limitations dubbed
of VFH+D is proposed, which considers
local obstacle
aAbstract:
different This
way
avoidance.
ofpaper
An enhanced
highlights
calculating somealgorithm
the obstacle limitations of the
dubbed
vector magnitude
VFH+
theVFH+D
VFH+ algorithm
is proposed,
algorithm
and a decay
on
algorithm
the
thefordomain
onwhich considers
domain
dynamic
of
of
local
alocal obstacle
different avoidance.
wayavoidance.
of calculating An enhanced
the obstacle algorithm dubbed
vector magnitude VFH+D is
and a decay proposed,
algorithm which considers
for considers
dynamic
obstacle obstacle
avoidance. An
Experiments enhanced algorithm
were conducted dubbed VFH+D
to compareand both is proposed,
algorithms which
on two different
a
a different
different
obstacle way
way of
avoidance.of calculating
calculating
Experiments the
the obstacle
obstacle vector
vector magnitude
were conducted magnitude
to compareand aa decay
both decay algorithm
algorithm
algorithms on two for
for dynamic
dynamic
different
mecanum
obstacle wheeled robots,
avoidance. VFH+D
Experiments wereachieved higher
conducted to average
compare speeds
both and lower distance
algorithms on two traveled
different
mecanum
obstacle wheeled robots,
avoidance. VFH+D achieved higher average speeds and lower distance traveled
to reach the
mecanum goal. robots, VFH+D achieved higher average speeds and lower distance different
wheeled
Experiments were conducted to compare both algorithms on two
to reach the
mecanum goal. robots, VFH+D achieved higher average speeds and lower distance traveled
wheeled traveled
to
to reach
reach the
Copyright the goal.The Authors. This is an open access article under the CC BY-NC-ND license
goal.
© 2020
(http://creativecommons.org/
Keywords: mobile robots, obstacle licenses/by-nc-nd/4.0)
avoidance, navigation, path planning, autonomous robotic
Keywords: mobile robots, obstacle avoidance, navigation, path planning, autonomous robotic
systems.
Keywords: mobile robots, obstacle avoidance, navigation,
navigation, path path planning,
planning, autonomous
autonomous robotic robotic
Keywords: mobile robots, obstacle avoidance,
systems.
systems.
systems.
1. INTRODUCTION are also the pure reactive Braitenberg inspired algorithms
1. INTRODUCTION are also the pure reactive Braitenberg inspired algorithms
1. INTRODUCTION (Braitenberg,
are also 1986) that will turn in the presence of static
1. INTRODUCTION
Obstacle avoidance algorithms are an essential aspect in are also the
(Braitenberg,
obstacles, thebut
pure
1986)
puresome
reactive
reactive
versions
Braitenberg
that will turn in the
Braitenberg
(Shayestegan
inspired
presence
inspired algorithms
of static
algorithms
and Marhaban,
(Braitenberg,
(Braitenberg,
obstacles, but 1986) 1986) that will turn in
that will (Shayestegan
some versions the presence
turn in the presence of
of static
and Marhaban, static
Obstacle
mobile robot
Obstacle avoidance algorithms
navigation,
avoidance as they
algorithms are
are an
allow
an essential
the robot
essential aspect
move-
aspect in
in 2012) alsobut implement additional rules to retake the path
Obstacle
mobile robot avoidance algorithms
navigation, as they areallow
an essential
the robot aspect
move- obstacles,
obstacles,
in 2012) alsobut some versions
some versions
implement (Shayestegan
(Shayestegan
additional and
rules to retake Marhaban,
and Marhaban,
the path
ment in the environment
mobile while preventing collisionsmove-
when 2012) to the also objective.
mobileinrobot
robot navigation,
navigation, while as
as they
they allow
allow the the robot
robot move- implement
implement additional
additional rules rules to to retake
retake thethe path
ment
following
ment in the
the environment
a path or achieving
environment while preventing
a goal. These collisions
preventing algorithms
collisions are 2012)
when
when
to the also
to the
objective.
objective.
path
ment in the
following a environment
path or achievingwhile a preventing
goal. These collisions
algorithms when
are More
to the advanced
objective. local planners have been developed that
used by the
following path planning
aa path methods, along with the robot More advanced local planners have been developed that
following
used by the path or or achieving
achieving aa goal.
goal. These
These algorithms
algorithms are
are can consider the local
kinematic, dynamic and other constraints
position
used by andpath
the the planning
path movement
planning methods,
area map,
methods, along
along to with the
calculate
with the robot
and More
robot advanced
can consider the local planners
kinematic, dynamichave been
and developed
other constraintsthat
used
positionby theandpath the planning
movement methods,
area map, along to with
calculate and More
the robot of
can the advanced
mobilethe
consider platforms planners
kinematic, while have fast
being
dynamic
been
and and developed
other robust inthat
constraints the
follow the best
position route in order to map,
reach to thecalculate
target while of the mobilethe platforms while being fast andand robust in the
positiontheandand the movement
theroute
movement area
area map, to calculate and
and can consider
obstacle
of the evasion
mobile
kinematic,
(Berns
platforms and
while
dynamic
von Puttkamer,
being fast
other
and
constraints
2009),
robust such
in as
follow best
avoiding obstacles.
follow This in order
behavior to reach
is the
essential target while
in multiple of the mobile
obstacle evasion platforms
(Berns and while vonbeing fast and2009),
Puttkamer, robustsuch in the
the
as
follow the
avoiding
best
best route
theobstacles. route This
in order
order to
in behavior reach
to is
reach the
the target
essential target
in
while
while obstacle
multiple the Dynamic evasionWindow
(Berns Approach
and von (DWA) (Fox2009),
Puttkamer, et al.,such
1997),as
navigationobstacles.
avoiding applications This such as cariscollision
behavior essential avoidance
in multiplefor obstacle
the Dynamic evasionWindow
(Berns Approach
and von (DWA) (Fox2009),
Puttkamer, et al.,such
1997),as
avoiding obstacles.
navigation applications Thissuch behavior
as car iscollision
essential in multiple
avoidance for the Timed Elastic
Dynamic Window Band (TEB) (DWA)
Approach (Roesmann (Fox et
et al.,
al., 2012),
1997),
automated applications
navigation road safety such (Adla as et
car al., 2013; avoidance
collision Dahl et al., for the Timed Elastic
Dynamic Window Band (TEB) (DWA)
Approach (Roesmann (Fox et
et al.,
al., 2012),
1997),
navigation
automated applications
road safety such (Adla as et
caral.,
collision
2013; avoidance
Dahl et al., for the FollowTimed the GapBand
Elastic Method(TEB) (FGM) (Sezer and
(Roesmann et Gokasan,
al., 2012),
2019), autonomous
automated road vehicles (Lefebvre et al., 2004; Houjie Follow the GapBand Method (FGM) (Sezer and Gokasan,
automated
2019), autonomousroad safety
safety
vehicles
(Adla
(Adla et
et al.,
(Lefebvreal., et2013;
2013;
al.,
Dahl et
et al.,
Dahl Houjie
2004; al., the2012)
the
Timed
and the
Follow
Elastic
the Vector
Gap Field
Method
(TEB)
Histogram
(FGM)
(Roesmann(VFH)
(Sezer
et(Borenstein
and
al., 2012),
Gokasan,
Jiang
2019), et al.,
autonomous 2016; Wang
vehicles et al., 2019),
(Lefebvre etunmanned
al., 2004; marine
Houjie the
2012) Follow
and the
the Gap
Vector Method
Field (FGM)
Histogram (Sezer
(VFH) and Gokasan,
(Borenstein
2019), et
Jiang autonomous
al., 2016; vehicles
Wang et (Lefebvre
al., 2019), etunmanned
al., 2004; marine
Houjie 2012) and Koren,and 1991)
the Vectoralgorithms.
Field All of these
Histogram (VFH) methods define,
(Borenstein
vessels (Chen
Jiang and Li, 2017), mobile robots, both individual and Koren, 1991) algorithms. All of these methods define,
Jiang et
vessels
al.,
al., 2016;
et(Chen 2016;
and
Wang
Wang
Li, 2017),
et al.,
al., 2019),
et mobile 2019),
robots,
unmanned
unmanned
both
marine
marine 2012)
individual in
and real andtime,
Koren,
the Vector
the
1991)
Fieldrobot
required
algorithms.
Histogram
All
(VFH)
velocities,
of these
(Borenstein
using
methods different
define,
(Miró et(Chen
vessels al., 2008;and Alajlan
Li, 2017), etmobile
al., 2015; Almasri
robots, both etindividual
al., 2016) and in real time,1991)
Koren, the required
algorithms. robotAll velocities,
of these using different
methods define,
vessels
(Miró et(Chen
al., 2008;and Li, 2017),
Alajlan etmobile
al., 2015;robots,
Almasri bothetindividual
al., 2016) strategies
in real time,to the
represent
required therobot
environment
velocities, from
using thedifferent
sensor
and foretcooperative
(Miró al., 2008; groups
Alajlan et (Soriano
al., 2015; et al., 2015;
Almasri et Alonso-
al., 2016) strategies
in real time,to the
represent
required therobot
environment
velocities, from
using thedifferent
sensor
(Miróforetcooperative
and al., 2008; Alajlan groups et (Soriano
al., 2015; et Almasri
al., et al.,
2015; 2016) strategies
Alonso- data, in order to to extractthe
represent from it the obstacle
environment from information
the
Morafor
and et cooperative
al., 2018). groups (Soriano et al., 2015; Alonso- data, strategiesin orderto represent
to extractthe fromenvironment
it the obstacle the sensor
frominformationsensor
and for
Mora et cooperative
al., 2018). groups (Soriano et al., 2015; Alonso- data, and use in it to determine
order to extract the
frombestit robot
the movement
obstacle to avoid
information
Mora et al., 2018). and use
data, in it to determine
order to extract the
frombestit robot
the movement
obstacle to avoid
information
Mora
In order et toal.,safely
2018). navigate the environment, path planning the and obstacles
use it to and transit the available free spaceto(Shim
In order
algorithms to safely
usually navigate
divide the
the environment,
problem in two path
main planning
compo- and
the
and use
obstacles
Kim, to determine
it 2018;determine
andCybulski
the
the
transit et
best
best
theal.,
robot
robot movement
available
2019). movement
free space
avoid
to(Shim
avoid
In order to
In order to safelysafely navigate
navigate the environment,
theproblem
environment, path
path planning
planning the obstacles and transit the available free space (Shim
algorithms
nents: A Global usually divide
navigation theplanner in
that two
uses main compo-
optimization the
and obstacles
Kim, 2018; and transit
Cybulski the
et al.,available
2019). free space (Shim
algorithms
algorithms
nents: A Global usually
usually divide
divide the
navigation the problem
problem
planner in two
two main
in uses
that main compo-
compo- and
optimization and Kim,
The Kim,
original 2018;
2018;VFH Cybulski
Cybulski et
et al.,
(Borenstein 2019).
al.,and
2019).Koren, 1991) searches
techniques
nents: A to obtain
Global navigation the best
planner routethattousesthe optimization
goal in the The original VFH (Borenstein and Koren, 1991) searches
nents:
techniquesA Global navigation
to obtain planner
the best routethattousesthe optimization
goal in the for The openings
original among(Borenstein
VFH obstacles that and are big enough
Koren, for the
1991) searches
available map
techniques to of the the environment (high levelgoalplanning)
techniquesmap
available to obtain
obtain
of the the best
best route
environment route to
to the
(high the
levelgoal in
in the
the for
planning)
The
robot
for
original
openings
to travel
openings
VFHand(Borenstein
among
among
obstacles
avoid and are
that
collisions,
obstacles that
Koren,
while
are big
1991) searches
bigbuilding
enough
enough
for the
a local
(Marder-Eppstein
available map of et al.,
the 2010; Pandey
environment (highet al.,
level 2017; Patle robot
planning) for openings
to travel among
and avoidobstacles that are
collisions, while enough for
bigbuilding for the
the
a local
available map of the
(Marder-Eppstein et environment
al., 2010; Pandey (highet level2017;
al., planning)
Patle map oftothe
robot encountered
travel and avoidobstacles
collisions, using
whilerangefinder
building sensors
a local
et al., 2019), and the
(Marder-Eppstein et Local
al., navigation
2010; Pandey planner,
et al., that gener-
2017; Patle map oftothe
robot encountered
travel and avoidobstacles
collisions, using
whilerangefinder
building sensors
a local
(Marder-Eppstein
et al., 2019), and the et Local
al., 2010; Pandeyplanner,
navigation et al., 2017; Patle to
that gener- map estimate
of the certainty of theirusing position in a grid. An
ates
et
et
velocity
al., 2019),
al.,velocity
ates
commands
and
and the
2019), commands the Local
Local
to follow
navigation
navigation
to follow
the global
planner,
planner,
the global
planthat
planthat
as closely
gener-
gener- map
as closely of the
to estimate
improvement the encountered
the certainty
encountered
of certainty
obstacles
of theirusing
obstacles
this algorithm, VFH+,
rangefinder
position
rangefinder
wasinproposed
sensors
in a grid.
sensorsAn
in
as possible, while considering the kinematic constraints to
to estimate
estimate the
improvement the
of certainty of
this algorithm, their
of their position
position
VFH+, a
a grid. An
grid.
wasinproposed An
in
ates velocity
atespossible,
velocity while commands
commands to follow
to followthe the global
thekinematic plan as closely
global planconstraints
as closely improvement
(Ulrich and Borenstein, 1998) thatVFH+, takes into consideration
as
andpossible,
as using thewhile sensor considering
data to dynamically
considering the kinematic detect improvement
changes (Ulrich
constraints of
and Borenstein,this algorithm,
of this algorithm,
1998) thatVFH+, takes intowas proposed
proposed in
wasconsideration in
as possible,
and using the while
sensor considering
data to the kinematic
dynamically constraints the robot size and improves the steer angle selection using
in the
and environment
using the sensor anddataobstacles
to in order detect
dynamically to avoid
detect changes
them (Ulrich
changes the robot
(Ulrich and
and Borenstein,
size and improves
Borenstein, 1998)
1998) thethat
that takes
steer into
angle
takes into consideration
selection using
consideration
and
in theusing the sensorand
environment data to dynamically
obstacles in order detect
to avoid changes
them athepolar robot histogram,
size that masks the angle
sectors behindusing the
(Miró et
in
in the
the et
al., 2008; and
environment
environment
Patleobstacles
and
et al., 2019).
obstacles in
in order
order
Among
to
to avoid
thethem
avoid
first the
them adetected
polar size and
robothistogram,
obstaclesand improves
improves
sothat
the
thenot
they masks
are
steer
steer
the angle
sectors
assigned
selection
selection
behind
as open using
the
space.
(Miró
and most al.,
simple 2008; Patle
obstacle et al.,
avoidance 2019).
methodsAmong the
usedthe first
as local a polar
adetected histogram,
polar histogram, that
that masks
masks the
the sectors
sectors behind
behind the
the
(Miró
(Miró et al.,
et al., 2008;
2008; Patle et
Patle avoidanceal., 2019).
et al., 2019). Among
Among the first
first detected obstacles
Another obstacles
improvement, so they are
VFH*, notwas assigned
proposed as open space.
in (Ulrich
and most
planners
and most simple
are obstacle
the bug
simple algorithm
obstacle avoidanceand its methods
variants
methods used as local
(Lumelsky
used as local detected obstacles
Another improvement, so they
so they are
VFH*, not
are not was assigned
assigned
proposed as open
as open space.
space.
in (Ulrich
and mostare
planners simple obstacle
the1987;
bug avoidance its
algorithm methods used as local and Borenstein, 2000) that defines a proposed
look aheadindirection
and Stepanov,
planners are Lumelsky and and its variants
Skewis, 1990;(Lumelsky
Ng and Another improvement,
and Borenstein, 2000) that VFH*,defineswas a proposed
look aheadindirection(Ulrich
planners
and Stepanov, are the bug
bug algorithm
the1987; Lumelsky and
algorithm and
and its variants
variants
Skewis, 1990;(Lumelsky
Ng and Another
(Lumelsky search
and withimprovement,
Borenstein, the A* 2000)
VFH*,
pathfinding
that defines
was
algorithm,
a look improving
ahead
(Ulrich
directionthe
Bräunl,
and 2007; Marı́n
Stepanov, 1987; etLumelsky
al., 2010)and thatSkewis,
avoid static
1990; obstacles
Ng and search
and with the A*
Borenstein, 2000) pathfinding
that defines algorithm,
a look improving
ahead directionthe
and Stepanov,
Bräunl, 2007; 1987;etLumelsky
Marı́n al., 2010) and
that Skewis,
avoid 1990;obstacles
static Ng and search obstaclewith detection
the A* inpathfinding
some cases, algorithm,
but also being closer to
improving a
the
by following
Bräunl, 2007; their
Marı́n perimeter
et al., untilthat
2010) theavoid
path static
is clear. search with
There obstacle
obstacles the A*inpathfinding
detection some cases, algorithm,
but also being improving
closer to the
a
Bräunl,
by 2007;their
following Marı́n et al., 2010)
perimeter until that
the avoid
path static
is obstacles
clear. There global path
obstacle planner
detection in without
some guaranteeing
cases, but also the optimality
being closer to a
by
 following their perimeter until the path is clear. There global
obstacle path planner
detection in without
some guaranteeing
cases, but also the
being optimality
closer to a
byThe
 following
financialtheirsupportperimeter
from theuntil the
University path
The financial support from the University of Costa Rica, under
is
of Costa clear.
Rica,There
under of the
global solution.
path planner without guaranteeing the optimality

theThe
grant 322-B8-298,
financial support is greatly
from appreciated. global
of the path
solution.planner without guaranteeing the optimality

theThe
grant 322-B8-298,
financial support from the
is greatly the University
University of
appreciated. of Costa
Costa Rica,
Rica, under
under of
of the
the solution.
solution.
the grant 322-B8-298, is greatly appreciated.
2405-8963 Copyright © 2020 The Authors. This is an open access article under the CC BY-NC-ND license.
the grant 322-B8-298, is greatly appreciated.
Peer review under responsibility of International Federation of Automatic Control.
10.1016/j.ifacol.2020.12.2450
 Daniel Díaz et al. / IFAC PapersOnLine 53-2 (2020) 9590–9595 9591

As the VFH+ is fast and robust in avoiding obstacles,

it is commonly used as a local planner (Yim and Park,
2014; Sary et al., 2018) in combination with other global
planning methods to provide a reliable solution to the
navigation problem (Pandey et al., 2017). The main disad-
vantage of the VFH+ algorithm is its inability to perform
well in environments with dynamic obstacles (Miró et al.,
2008). In order to overcome this limitation, the algorithm
is usually combined with other techniques, such as the
Kalman filter (Guo et al., 2010; Kim et al., 2010), in order
to predict obstacle movement, the aforementioned showing
promising results but demonstrated only by simulations. Fig. 1. Trail left by moving obstacle near a wall on the
In this work we analyze some of the limitations of the occupancy grid, darker cells have higher occupancy
VFH+ algorithm and propose a new and improved version values
called Vector Field Histogram + Dynamic (VFH+D) that
overcomes these deficiencies. The proposed algorithm has
the capacity of avoiding dynamic obstacles, while also
improving the robot speed and success rate in reaching
the goal point while avoiding collisions, as shown by
the experiments conducted on an omnidirectional robot
that implements the proposed algorithm in the Robot
Operating System (ROS).

2. VFH+ ALGORITHM AND LIMITATIONS

The VFH+ algorithm as defined on Ulrich and Borenstein Fig. 2. Several grid cells with maximum occupancy value
(1998) uses five data structures, the data is processed near a wall, red dots are the actual laser ranger
sequentially from one data structure to the next: two readings at a given time.
2-dimensional grids and three 1-dimensional polar his-
tograms. The following is a summary of the data flow. The steering direction is determined by applying a
cost function to all candidate directions and selecting
• The map grid or obstacle grid C: a 2-dimensional grid the one with the least cost.
that represents the obstacles in the world reference
frame, each cell holds a certainty value between 0 and mi,j = c2i,j (a − b · d2i,j ) (1)
cmax . A cell’s certainty value is increased by 1 for each
sensor reading that detects an obstacle in that cell. For equation (1), Borenstein suggests choosing the param-
• The active window Ca : a much smaller 2-dimensional eters a and b such that mi,j in border cells is equal to
grid that follows the robot. Each cell holds an “ob- c2i,j .
stacle vector” that consists of a magnitude mi,j and While working with the VFH+ algorithm in practice
direction βi,j , where mi,j is a function of the cells several limitations became evident. The first was the
distance to the robot’s center di,j and corresponding inability of the algorithm to handle moving obstacles.
certainty value ci,j as described by (1), and βi,j is the Grid cells in the obstacle grid are updated to increase
direction from the robot’s center to the cell. their certainty values when objects are detected, however,
• The primary polar histogram H p : a 1-dimensional no mechanism for decreasing these same cells values is
histogram of the angular sectors of width α around described. As such, in situations where there are moving
the robot. Each sector holds a polar obstacle density obstacles the algorithm could deem large parts of the grid
which is the sum of the magnitude of all the cells in to be occupied, as shown in Fig. 1. The cells are actually
Ca that fall within that sector. An enlargement angle free, but the “trails” of moving obstacles were left in the
for cells is also defined based on the robot’s radius rr occupancy grid.
and a parameter for minimum obstacle distance rs ,
so a single cell can add to more than one sector. Another related problem happens when the robot’s odom-
• The binary polar histogram H b : a 1-dimensional etry has poor accuracy or is badly configured, or when the
histogram that maps each sector on H p to 0 (free) robot’s wheels slip. Differences between the approximated
or 1 (blocked) based on its value Hkp . Two thresholds and actual position can cause the occupied cells to “blur”
τlow and τhigh are defined, if Hkp < τlow then Hkb = 0, into adjacent cells as demonstrated in Fig. 2.
if Hkp > τhigh then Hkb = 1, otherwise Hkb remains In addition to the former, additional tests also brought
unchanged from its previous value. other undesirable behaviour to light. Many of our tests
• The masked polar histogram H m : additional sectors were done in a relatively small (7 × 5 m) enclosed arena.
in H b are blocked based on the robot’s direction of We noticed that the algorithm tended to perform poorly
movement and minimum steering radius. close to the walls, especially when the direction of move-
• Consecutive free sectors in H m are classified as wide ment was perpendicular to the wall. Also, the robot often
or narrow valleys according to their size, and candi- avoided walls that were far away as much as obstacles that
date directions for each valley are then added to a list. were right next to it, adjusting parameters didn’t seem to
9592 Daniel Díaz et al. / IFAC PapersOnLine 53-2 (2020) 9590–9595

correct this. After analyzing the effects of the obstacle grid Algorithm 1 Decay algorithm
on the polar histogram for different parameter configura-
Require: d: decay value, gb: decay guard band.
tions the team came up with some hypotheses to explain
Ensure: C: obstacle grid, Cs : obstacle grid size, ws : active
this behaviour.
window size, wc : ws /2, i0 : robot’s X position in C,
First, the sensors used for obstacle detection should be j0 : robot’s Y position in C.
considered, in our case, scanning laser rangefinders in the 1: procedure DecayActiveWindow(d, gb)
front and back of the robot. In general, cells tended to 2: is ← max(i0 − (wc + gb), 0)
become saturated in regards to their occupancy value very 3: js ← max(j0 − (wc + gb), 0)
quickly (in less than a second), such that all cells had 4: if ← min(is + ws + 2 · gb), Cs )
either the maximum occupancy value or zero. In turn, 5: jf ← min(js + ws + 2 · gb), Cs )
the magnitude of cells in the active window was 6: for i ← is , if do
usually the maximum value possible given the cells 7: for j ← ji , jf do
distance as described by equation (1). This is due 8: if C[i][j] ≥ d then
to the high sampling rate and resolution of the laser 9: C[i][j] ← C[i][j] − d
rangefinders sensors used. 10: else
11: C[i][j] ← 0
Since the polar obstacle density of each circular sector is 12: end if
calculated as the sum of the magnitudes of all cells inside 13: end for
that sector, and cells are arranged in a simple square grid, 14: end for
there are a lot more cells that contribute to the value of a 15: end procedure
sector further away from its origin. Because of this and the
way H p is calculated with (1), larger obstacles that are far
3.2 Magnitude equation
away and closer to the border of the active window tend to
drive the polar obstacle density of a single sector to values
as high or higher than smaller obstacles much closer to the The second proposed change to the algorithm is the use of
robot, this is considered as an undesired behaviour. a different equation to determine obstacle magnitude, in
order to improve on the poor performance of equation (1)
The former situation is exemplified in Figs. 3, 4 and 5, in cluttered environments.
which correspond to a configuration with a wall to the left
and an obstacle to the right of the robot (the white cells are While considering alternatives, we minded the following
occupied by the robot, the black cell is the robot’s center). important points:
Notice in Fig. 4, the magnitude of obstacle vectors in the • Distance should have a higher impact on vector
active window is lower on cells farther from the robot, as magnitude, a linear or quadratic proportion is not
expected from equation (1). However, the polar histogram enough.
has a higher obstacle density in the direction of the wall, • Cells with the maximum occupancy value in close
as show in Fig. 5. vicinity to the robot should have a blocking effect
on the polar histogram, even if it is just one or two
3. PROPOSED ALGORITHM cells.
• The equation’s different parameters should affect in-
For the purpose of this paper, the aforementioned prob-
dependent aspects of the observed behaviour of the
lems can be summarized as follows:
robot.
(1) The algorithm is not robust to changes or
After considering the former aspects, we came up with
uncertainties in the obstacle grid, and cannot
equation (2). Let’s assume c2i,j = 1 in order to focus on
deal with moving obstacles.
the exponential term, which is what really differentiates
(2) The algorithm performs poorly in the presence
this new function from equation (1). Figure 6 plots m(d)
of walls, and in general obstacle proximity does
for different E and B values, these will be the main
not translate to a higher obstacle density in H p.
parameters to adjust the shape of the function.
 di,j E
3.1 Obstacle decay rate 2
1
−B · D
mi,j = ci,j · e (2)
The first proposed change to the VFH+ algorithm is to
add a “decay” to cells around the active window, such that Notice how higher E values produce a more pronounced
occupancy values of all the cells inside the window plus an slope. In practice, the E value should be increased if the
additional number of surrounding “guard band” cells will algorithm is not making enough distinction between close
decrease over time. We define the following parameters: and distant obstacles. On the other hand, higher B values
“spread” the slope along the x-axis. In practice, this would
• Decay rate (Rd ): the frequency with which occu- be done to increase the minimum distance to obstacles that
pancy values are decreased. determines a particular direction to be blocked.
• Decay value (d): the amount to subtract from each
Finally, parameter D is useful to “normalize” or change the
cell.
function’s scale on the x-axis and allow portability across
• Decay guard band (gb): additional cells to decay
different robots or use cases, for example by setting D to
around the active window.
the robot’s radius rR . Other values that could be used are
The main control loop will then periodically call algo- the width of the active window, the LIDAR’s maximum
rithm 1 at the defined rate. range or some other significant reference value. In fact,
 Daniel Díaz et al. / IFAC PapersOnLine 53-2 (2020) 9590–9595 9593

Fig. 3. Active window occupancy Fig. 4. Active window magnitude Fig. 5. Polar histogram

E=2.3 B=1.0
1
E=3.2 B=4.0
E=4.1 B=16.0

0.8

0.6
Magnitude

0.4

0.2
Fig. 7. Obstacle course and robot B
0
Table 1. Dimensions of the robots tested
0 0.5 1 1.5 2 2.5 3
Distance la lb r
Robot A 29.41 cm 21.03 cm 10.15 cm
Fig. 6. Effect of parameters B and E on equation (2). Robot B 15.5 cm 12.5 cm 3 cm

parameter D is actually redundant, but we found it to be Table 2. Success rate on reaching the goal
very useful to simplify parameter tuning in practice.
Also, notice how m(d) stays within the range [0, 1[, re- Algorithm Robot Trials Success Rate
gardless of which values are selected. This also facilitates Robot A 10 30%
VFH+
the parameter tuning process. For example, we used B = Robot B 10 100%
16.31, E = 3.2, and set D to rR . In Fig. 6, this would Robot A 10 100%
closely resemble the dashed green line, with the x-axis VFH+D
Robot B 10 100%
normalized to x times the Robot radius rR .
Now, we could say that any obstacle closer than 1.5 times on the other hand O2 and O3 leave enough space for the
rR to the robot’s center will have almost the maximum robot to pass. Robot B, being much smaller, easily fits
magnitude, while obstacles more than 3 times rR away through both gaps.
from the robot’s center will be negligible. Selecting the Ten trials were conducted for each combination of robot
threshold values τlow and τhigh is also easier. We can define and algorithm as shown in table 2, with the same initial
them solely in terms of c2max , e.g. set the thresholds around conditions for all the tests. For the VFH+ algorithm with
4 · c2max to specify that there should more than 4 occupied Robot A, only 3 out of 10 trials managed to reach the
cells in one particular direction to block it. goal. Table 3 summarizes the results, excluding those trials
where the robot could not reach the goal. We measured the
4. EXPERIMENTAL RESULTS Integral Absolute Error by defining the error (t) as the
robot’s Euclidean distance to the goal. Figures 8 and 9
In order to evaluate the performance of the new algorithm show the path followed on one instance of each algorithm
we used the obstacle configuration shown in Fig. 7. The for robot A. Figures 10 and 11 do the same for robot B.
robot’s goal was set 3.5 m ahead of its starting position. We
Our results suggest that the proposed VFH+D algorithm
measured the robot’s position using an OptiTrack motion
can have a higher success rate at traversing cluttered
capture system. The tests were done on two different
indoor environments. We believe this is mainly due to the
Mecanum wheeled mobile robots, the robots’ dimensions
improved mapping between obstacle distance and polar
are summarized on table 1. For more information on the
obstacle density. This allows the robot to correctly ignore
robots used refer to Röhrig et al. (2010) and Marı́n (2018).
obstacles that are too far away to impose restrictions on
The parameters for VFH+D were selected using the afore- its movement. This was particularly hard to accomplish
mentioned guidelines. In this configuration, the gap be- for VFH+, without considerably reducing the size of the
tween O1 and O2 is too small for Robot A to pass through, active window.
9594 Daniel Díaz et al. / IFAC PapersOnLine 53-2 (2020) 9590–9595

Table 3. Algorithm performance comparison

Distance traveled [m] Average speed [m/s] IAE [m·s]

Algorithm Robot
min avg max stdev min avg max stdev min avg max stdev
VFH+ 1 A 4.71 5.32 5.84 0,57 0.128 0.213 0.262 0,074 33.12 41.24 48.17 7,59
VFH+D A 4.71 4.85 4.97 0,09 0.165 0.284 0.336 0,047 30.58 31.28 32.29 0,49
VFH+ B 3.84 3.86 3.89 0,02 0.094 0.217 0.253 0,047 28.89 30.00 36.25 2,28
VFH+D B 3.71 3.78 4.15 0,13 0.264 0.277 0.306 0,011 26.31 27.15 27.94 0,51

3 3 tuning VFH+ to allow it to somewhat successfully clear

the obstacle course, we found that H p needs at least 160
Path Path
2.5 Goal 2.5 Goal
Starting point Starting point
2 O1 2 O1

1.5
O2
O3 1.5
O2
O3
sectors, while our parameterization of VFH+D required
1 1
only 100 sectors to successfully reach the goal on every
y (m)

y (m)

0.5 0.5

0 0 iteration. Intuitively, we interpret this as a better or more

-0.5

-1
-0.5

-1
efficient codification of the information necessary to cor-
-1.5 -1.5
rectly decide which path to follow.
-2 -2
-0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
x (m) x (m)

5. CONCLUSIONS
Fig. 8. VFH+ algorithm, Fig. 9. VFH+D algorithm,
Robot A Robot A This paper presents VFH+D, an improvement on VFH+
3 3
for local obstacle avoidance. The proposed changes are:
Path Path
2.5 Goal 2.5 Goal

2
Starting point
O1
O2
2
Starting point
O1
O2
• The introduction of cell occupancy decay, which al-
lows for dynamic obstacle avoidance (algorithm 1).
O3 1.5 O3
1.5

1 1

• A new obstacle vector magnitude equation for cells in

y (m)
y (m)

0.5 0.5

-0.5
0

-0.5
the active window, as described by (2).
-1 -1

-1.5
-1.5 Also, it was shown that the parameter tuning for VFH+D
-2
-0.5 0 0.5 1 1.5 2
x (m)
2.5 3 3.5 4 4.5
-2
-0.5 0 0.5 1 1.5 2
x (m)
2.5 3 3.5 4 4.5
is more intuitive and requires less iterations.
To measure the new algorithm’s performance, a total of 40
Fig. 10. VFH+ algorithm, Fig. 11. VFH+D algo- trials with 2 different robots on one obstacle course with
Robot B rithm, Robot B static obstacles were conducted. The results show that the
As for the trials where VFH+ could not reach the goal, we new algorithm incurred in lower average distance traveled
observed that the robot went for the opening between O2 to reach the goal and higher average speed, and achieved
and O3 , but as it got closer that direction became blocked. consistently a lower IAE. For one of the robots tested,
In turn the robot backed up, until the obstacles were far VFH+ failed to reach the goal on 7 out of 10 trials, while
away enough that once again the direction between O2 and VFH+D reached the goal on every iteration.
O3 was unblocked, and the cycle repeated. Effectively, the
robot was stuck in a “local minimum”. Once the robot has REFERENCES
failed in its first attempt to clear this opening, it’s very
Adla, R., Al-Holou, N., Murad, M., and Bazzi, Y.A.
unlikely that any further attempt will succeed, because
(2013). Automotive collision avoidance methodologies
of the blurring effect explained in section 2 and Fig. 2.
sensor-based and its-based. In 2013 ACS Interna-
We attributed the initial success or failure of the runs
tional Conference on Computer Systems and Applica-
to slight variations in quantization of the obstacles’ and
tions (AICCSA), 1–8.
robot’s position to the discrete data structures of the grid.
Alajlan, A.M., Almasri, M.M., and Elleithy, K.M. (2015).
VFH+D also achieved a higher average speed on both Multi-sensor based collision avoidance algorithm for
robots. This may be expected, if the speed is calculated as mobile robot. In 2015 Long Island Systems, Applications
a function of the obstacle density in the robot’s direction and Technology, 1–6.
of movement (higher density means lower speed). Given Almasri, M.M., Alajlan, A.M., and Elleithy, K.M. (2016).
that the obstacle density for VFH+D is lower for far away Trajectory planning and collision avoidance algorithm
obstacles, the robot’s movement speed when traveling in for mobile robotics system. IEEE Sensors Journal,
the direction of such obstacles will be higher. This is often 16(12), 5021–5028.
the case for indoor environments, where walls are present Alonso-Mora, J., Beardsley, P., and Siegwart, R. (2018).
in every direction and frequently fall within the active Cooperative collision avoidance for nonholonomic
window. robots. IEEE Transactions on Robotics, 34(2), 404–420.
Berns, K. and von Puttkamer, E. (2009). Autonomous
Additionally, VFH+ as defined in Ulrich and Borenstein Land Vehicles. Vieweg+Teubner Verlag.
(1998) cannot deal with moving obstacles without becom- Borenstein, J. and Koren, Y. (1991). The vector field
ing trapped. The addition of decay to the active window histogram-fast obstacle avoidance for mobile robots.
allows VFH+D to escape such situations. IEEE Transactions on Robotics and Automation, 7(3),
Another aspect which is not immediately obvious from our 278–288.
results is that VFH+D allows for better overall perfor- 1 For VFH+ with robot A, includes only the 3 trials that reached

mance with smaller data structures. For example, while the goal
 Daniel Díaz et al. / IFAC PapersOnLine 53-2 (2020) 9590–9595 9595

Braitenberg, V. (1986). Vehicles: Experiments in synthetic Ng, J. and Bräunl, T. (2007). Performance comparison of
psychology. MIT press. bug navigation algorithms. Journal of Intelligent and
Chen, Y. and Li, T. (2017). Collision avoidance of Robotic Systems, 50(1), 73–84.
unmanned ships based on artificial potential field. In Pandey, A., Pandey, S., and Parhi, D. (2017). Mobile
2017 Chinese Automation Congress (CAC), 4437–4440. robot navigation and obstacle avoidance techniques: A
Cybulski, B., Wegierska, A., and Granosik, G. (2019). review. International Robotics & Automation Journal,
Accuracy comparison of navigation local planners on 2(3), 00022.
ros-based mobile robot. In 2019 12th International Patle, B., L, G.B., Pandey, A., Parhi, D., and Jagadeesh,
Workshop on Robot Motion and Control (RoMoCo), A. (2019). A review: On path planning strategies for
104–111. navigation of mobile robot. Defence Technology, 15(4),
Dahl, J., de Campos, G.R., Olsson, C., and Fredriksson, 582 – 606.
J. (2019). Collision avoidance: A literature review on Roesmann, C., Feiten, W., Woesch, T., Hoffmann, F., and
threat-assessment techniques. IEEE Transactions on Bertram, T. (2012). Trajectory modification consid-
Intelligent Vehicles, 4(1), 101–113. ering dynamic constraints of autonomous robots. In
Fox, D., Burgard, W., and Thrun, S. (1997). The dynamic ROBOTIK 2012; 7th German Conference on Robotics.
window approach to collision avoidance. IEEE Robotics Röhrig, C., Heß, D., Kirsch, C., and Künemund, F. (2010).
Automation Magazine, 4(1), 23–33. Localization of an omnidirectional transport robot us-
Guo, J., Zhang, S., Xu, J., and Zhou, S. (2010). Kalman ing ieee 802.15.4a ranging and laser range finder. In
prediction based VFH of dynamic obstacle avoidance for 2010 IEEE/RSJ International Conference on Intelligent
intelligent vehicles. In 2010 International Conference on Robots and Systems, 3798–3803.
Computer Application and System Modeling (ICCASM Sary, I.P., Nugraha, Y.P., Megayanti, M., Hidayat, E., and
2010), volume 3, V3–6–V3–10. Trilaksono, B.R. (2018). Design of Obstacle Avoidance
Houjie Jiang, Zhuping Wang, Qijun Chen, and Jin Zhu System on Hexacopter Using Vector Field Histogram-
(2016). Obstacle avoidance of autonomous vehicles with Plus. In 2018 IEEE 8th International Conference on
cqp-based model predictive control. In 2016 IEEE Inter- System Engineering and Technology (ICSET), 18–23.
national Conference on Systems, Man, and Cybernetics Sezer, V. and Gokasan, M. (2012). A novel obstacle
(SMC), 001668–001673. avoidance algorithm: “follow the gap method”. Robotics
Kim, D., Kim, J., Bae, J., and Soh, Y. (2010). Devel- and Autonomous Systems, 60(9), 1123 – 1134.
opment of an Enhanced Obstacle Avoidance Algorithm Shayestegan, M. and Marhaban, M.H. (2012). A braiten-
for a Network-Based Autonomous Mobile Robot. In berg approach to mobile robot navigation in unknown
2010 International Conference on Intelligent Computa- environments. In S.G. Ponnambalam, J. Parkkinen, and
tion Technology and Automation, volume 2, 102–105. K.C. Ramanathan (eds.), Trends in Intelligent Robotics,
Lefebvre, O., Lamiraux, F., Pradalier, C., and Fraichard, Automation, and Manufacturing, 75–93. Springer Berlin
T. (2004). Obstacles avoidance for car-like robots Heidelberg.
integration and experimentation on two robots. In IEEE Shim, Y. and Kim, G.W. (2018). Range sensor-based
International Conference on Robotics and Automation, efficient obstacle avoidance through selective decision-
2004. Proceedings. ICRA ’04. 2004, volume 5, 4277– making. Sensors, 18(4).
4282 Vol.5. Soriano, A., Bernabeu, E.J., Valera, A., and Vallés, M.
Lumelsky, V. and Skewis, T. (1990). Incorporating range (2015). Collision avoidance method based on consensus
sensing in the robot navigation function. IEEE Transac- among mobile robotic agents. International Journal of
tions on Systems, Man, and Cybernetics, 20, 1058–1068. Imaging and Robotics, 15(1), 80–90.
Lumelsky, V. and Stepanov, A.A. (1987). Path planning Ulrich, I. and Borenstein, J. (1998). VFH+: reliable
strategies for a point mobile automaton moving amidst obstacle avoidance for fast mobile robots. In Proceed-
unknown obstacles of arbitrary shape. Algorithmica, 2, ings. 1998 IEEE International Conference on Robotics
403–430. and Automation (Cat. No.98CH36146), volume 2, 1572–
Marder-Eppstein, E., Berger, E., Foote, T., Gerkey, B., 1577 vol.2.
and Konolige, K. (2010). The office marathon: Ro- Ulrich, I. and Borenstein, J. (2000). VFH*: local ob-
bust navigation in an indoor office environment. In stacle avoidance with look-ahead verification. In Pro-
2010 IEEE International Conference on Robotics and ceedings 2000 ICRA. Millennium Conference. IEEE In-
Automation, 300–307. ternational Conference on Robotics and Automation.
Marı́n, L. (2018). Modular open hardware omnidirectional Symposia Proceedings (Cat. No.00CH37065), volume 3,
platform for mobile robot research. In 2018 IEEE 2505–2511 vol.3.
2nd Colombian Conference on Robotics and Automation Wang, P., Gao, S., Li, L., Sun, B., and Cheng, S.
(CCRA), 1–6. (2019). Obstacle avoidance path planning design for
Marı́n, L., Valles, M., Valera, A., and Albertos, P. (2010). autonomous driving vehicles based on an improved ar-
Implementation of a bug algorithm in the e-puck from tificial potential field algorithm. Energies, 12(12).
a hybrid control viewpoint. In 2010 15th International Yim, W.J. and Park, J.B. (2014). Analysis of mobile robot
Conference on Methods and Models in Automation and navigation using vector field histogram according to the
Robotics, 174–179. number of sectors, the robot speed and the width of the
Miró, J.V., Taha, T., Wang, D., and Dissanayake, G. path. In 2014 14th International Conference on Control,
(2008). An adaptive manoeuvring strategy for mobile Automation and Systems (ICCAS 2014), 1037–1040.
robots in cluttered dynamic environments. IJAAC, 2,
178–194.