HAL Id: inria-00211845
https://hal.inria.fr/inria-00211845
Submitted on 22 Jan 2008
HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entic research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.
L’archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diusion de documents
scientiques de niveau recherche, publiés ou non,
émanant des établissements d’enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.
Combining Probabilistic Velocity Obstacles and
Occcupancy Grid for safe Navigation in dynamic
environments
Chiara Fulgenzi, Anne Spalanzani, Christian Laugier
To cite this version:
Chiara Fulgenzi, Anne Spalanzani, Christian Laugier. Combining Probabilistic Velocity Obstacles and
Occcupancy Grid for safe Navigation in dynamic environments. Workshop on safe Navigation, IEEE
ICRA, Apr 2007, Rome, Italy. �inria-00211845�
Dynamic Obstacle Avoidance in uncertain environment combining
PVOs and Occupancy Grid
Chiara Fulgenzi, Anne Spalanzani, and Christian Laugier
Laboratoire d’Informatique de Grenoble, INRIA Rh
ˆ
one-Alpes, France
Email: firstname.lastname@inrialpes.fr
Abstract— Most of present work for autonomous navigation
in dynamic environment doesn’t take into account the dynamics
of the obstacles or the limits of the perception system. To face
these problems we applied the Probabilistic Velocity Obstacle
(P V O) approach [1] to a dynamic occupancy grid. The paper
presents a method to estimate the probability of collision where
uncertainty in position, shape and velocity of the obstacles,
occlusions and limited sensor range contribute directly to the
computation. A simple navigation algorithm is then presented
in order to apply the method to collision avoidance and goal
driven control. Simulation results show that the robot is able
to adapt its behaviour to the level of available knowledge and
navigate safely among obstacles with a constant linear velocity.
Extensions to non-linear, non-constant velocities are proposed.
I. INTRODUCTION
Mobile robots navigation in dynamic environments rep-
resents still a challenge for real world applications. The
robot should be able to gain its goal position navigating
safely among moving people or vehicles, facing the implicit
uncertainty of the surrounding world and the limits of its
perception system.
The problem of autonomous navigation has been deeply
studied in literature and several techniques have been de-
veloped. The global approaches (path planning algorithms)
compute a complete path from the robot actual position to
the goal [2]. In the case of moving obstacles, a common
technique is to add the time dimension to the state space
and reduce the problem to a static one [3]. Also if global
methods can provide optimal solutions, their major drawback
is that they assume a complete and deterministic knowledge
of the environment: in practical applications they are usually
combined with local methods in order to avoid unexpected
obstacles [4], [5]. These l ast ones, also called reactive
methods, generate just the next input control: they use only
the nearest portion of the environment and update the world
model according to the current sensor observation. Most of
the developed t echniques, as the Dynamic window approach
[4], [6], the curvature velocity [7] and the lane curvature
method [8] don’t take into account the dynamic information
of the environment, considering all the obstacles as static
ones. On the other side, the Velocity Obstacles approach
[9], [10], the Inevitable Collision States concept [11] and
[12] use a deterministic knowledge about the velocity of the
obstacles to compute collision-free controls. All the cited
Fulgenzi Chiara is supported by a grant from the European Community
under the Marie-Curie project VISITOR MEST-CT-2004-008270
methods however, rely on a complete knowledge of the static
and dynamic environment and a deterministic representation
of the world.
In this paper we propose a reactive obstacle avoidance based
on a probabilistic framework such to make the connection
between the perception and the navigation system of the
robot. In [1], the Probabilistic Velocity Obstacle approach
(PVO) has been proposed as an extension of the VOs to
the case of uncertain estimation of velocity and of the
radius of circular obstacles. We combined this method with
the dynamic occupancy grid provided by a general sensor
system. The hypotheses on the robot and obstacle shape are
removed. The sensors provide a probabilistic estimation of
the occupied and free space around the robot and of the
velocity with which t he objects are moving; the observations
update a 4D probabilistic occupancy grid (space and veloc-
ity) [13]; the probability of collision in time is estimated for
each reachable velocity of the robot. A simple navigation
algorithm is also proposed in order to apply the best control
with respect to safety issues and convergence to the goal.
Simulation results show how the developed algorithm takes
directly into account limited range and occlusions, uncertain
estimation of velocity and position of the obstacles, allowing
the robot to navigate safely toward the goal and to modify
its behaviour according to the quality of its perception.
The paper is structured as follows: in Section II the Bayesian
Occupancy Filter (BOF) and the Velocity Obstacle frame-
work are recalled and discussed; in Section III the developed
solution is described in detail. In Section IV simulation re-
sults are shown and discussed. Section V closes the document
with remarks and purposes for future activities.
II. RELATED WORKS
The method here developed combines two existing frame-
works: the Bayesian Occupancy Filter [14] and the Lin-
ear Velocity Obstacles [9]. The Bayesian Occupancy Filter
(BOF) is a dynamic occupancy grid where an estimation of
velocity is stored as well as the probability of occupation.
Sensor observations are processed from the BOF and the
resulting grid is given as input to the obstacle avoidance
algorithm. The following paragraphs recall respectively the
BOF algorithm and the Linear Velocity Obstacles approach.
Paragraph II -C discusses t he advantages of the combination
of the two methods.
(a) (b) (c)
Fig. 1. Simulated detection of two cars crossing each others. (a) Simulated environment : the robot equipped with a laser range finder detects a car
moving from left to right and a second car moving from right to left. (b) Dynamic occupancy grid: red is high, blue is low probability of occupation. The
space behind the cars has low probability of occupation. (c) Clustering: different colou rs characterise objects and occluded or free space.
A. The Bayesian Occupancy filter (BOF)
Probabilistic occupancy grids are well known structures
used for environmental representation. The space is divided
in a finite number of cells, each representing a position in
the 2D plane (X, Y ), X = [
x
q
], Y = [
y
q
], where q is the
discretization step. The estimation of the state of the system
x(t) at t ime t is the list of the states of all the cells of
the grid: Occ, when the cell is occupied or Emp if the
correspondent space is free. Given a probabilistic sensor
model P (z(t)|x(t)) where z(t) is the current observation,
the grid is updated following the Bayes rule. Under the hy-
pothesis that each cell of the grid is statistically independent
from its neighbourhood, each cell state estimation is updated
independently [15].
If some moving obstacles is present, the precedent structure
is not sufficient to describe the state of the environment
and it is necessary to introduce a description of velocity
and a dynamical model. To perform an estimation, the
state of the grid is firstly modified according the dynamical
model (prediction step) and then compared with the acquired
observation ( updating step). These ideas are at the basis of
the Bayesian Occupancy Filter [13]. Each cell maintains not
only an estimation of its occupation probability, but also
a discretized representation of the probabilistic distribution
function (pdf) over velocities. A minimum and maximum
velocity value is considered for eventual objects in the space,
so that the pdf is given by a finite histogram over velocity
values v
n
with n = 1...N. The discrete approximation is
performed according to the spatial and time discretization:
given τ the time step, only integer velocities in terms of
q
τ
are taken under consideration, in order to perform fast and
rigorous prediction and updating steps. Here we present a
brief scheme of the algorithm:
1) At the beginning of the estimation, the occupancy
grid is initialised with the prior knowledge of the
environment: if no knowledge is available all the cells
are initialised with a 0.5 probability of occupation and
a uniform distribution over velocities;
2) A prediction step is performed according to the state
of the environment and a constant velocity dynamical
model. For each cell c = [i, j] and for each value of
velocity v
n
= [di, dj], an antecedent cell is considered
: c
a
(n) = [i − di, j − dj]. Under the hypothesis that
each cell is independent, the predicted occupation of
each cell is computed as follows:
ˆ
P
c
(Occ) =
X
n
P
c
a
(n)
(Occ) · P
c
a
(n)
(v
n
) (1)
The predicted probability distribution function of ve-
locity of a cell c is obtained by a normalisation over
all velocity probability values P (v
n
) of each c
a
(n);
3) Sensor data are acquired and an observed occupation
grid is built according to the probabilistic observation
model;
4) The grid is updated following the Bayes rule:
P
c
(Occ|z(t)) ∝ P
c
(z(t)|Occ) ·
ˆ
P (Occ) (2)
5) The grid is searched for clusters: first the 4-connection
recursive algorithm is applied, than each cluster is
checked for coherent velocity profiles. In case of two or
more groups of cells with coherent velocity, the cluster
is divided again. Each cell is given a cluster index and a
velocity profile for each cluster is calculated according
to the estimation of each cell;
6) Back to step 2.
For further details the interested reader may refere to the
original papers [14]. Fig. 1(a) shows a simulated environ-
ment: the cycab is equipped with a laser range finder and
perceives two cars: the nearest moving from left to right
and another just behind, moving from right to l eft. Fig.
1(b) s hows the dynamic occupancy grid computed: red stays
for high probability of occupation, while blue is for low
probability. Fig. 1(c) shows the clusters found on the grid,
correspondent to the two cars.
B. Linear Velocity Obstacle
Here we describe the classical approach to VO in terms
that could help the understanding of the cell-to-cell approach;
the original algorithm has been introduced by Fiorini and
Shiller in [9].
Fig. 2. Collision Cone for a punctual robot and a circular obstacle with
linear velocity v
o
; v
r
is in collision.
Lets consider a punctual robot r in [x
r
, y
r
] free to move
in the 2D plane, and an obstacle o of arbitrary shape, with
centre in [x
o
, y
o
] and constant linear velocity v
o
. With t his
definition of the robot, the configuration space ( i.e. the
space where each point corresponds to a configuration of the
robot and obstacles correspond to configurations in collision)
is equivalent to the Euclidean space. The velocity space is
defined as the configuration space where linear velocities are
described by vectors attached to the centre of objects. The
idea is to work directly in this space and determine the set of
all linear velocities that lead the robot to a collision in future
time (Velocity Obstacle). Let’s then define the Collision Cone
CC
ro
of the robot r relative to the obstacle o as the set of
all relative velocities v
′
r
= (v
r
− v
o
) that leads the robot in
collision with o in future time:
CC
ro
=
n
v
′
r
|∃t > 0, (x
r
+ v
′
r
−→
i × t, y
r
+ v
′
r
−→
j × t) ∈ o
o
(3)
where
−→
i and
−→
j are the unity vectors of x and y
directions, respectively.
The CC
ro
is the positive angle with vertex in (x
r
, y
r
)
and rays the right and left tangent to object o . To know i f a
velocity v
r
is in collision with the obstacle o it is sufficient
to consider the relative vector v
′
r
= (v
r
− v
o
) and to verify
if it points in the CC
ro
, i.e. check if the extension of the
vector i n the positive dir ection intercepts the obstacle.
The velocity obstacle V O
ro
is obtained translating CC
ro
by the obstacle velocity v
o
: all and only the velocities v
r
pointing outside the cone are collision free. If more than
one obstacle is present in the environment, it is sufficient to
consider the union of each velocity obstacle [9] :
V O
r
=
[
k=1...K
V O
ro
k
(4)
If the robot is circular with centre in (x
r
, y
r
) , the corre-
sponding configuration space is given by a punctual robot
in (x
r
, y
r
) and all the obstacles enlarged by the radius
of the robot. Under the hypothesis of circular robot and
obstacles, uncertainty in radius and in velocity can be taken
into account [1].
C. Discussion on the chosen methods
To perform a safe navigation in a unknown or partially
known dynamic environment, the mobile robot has to rely
on a feasible representation of the world constantly updated
according to the sensor observations and that allows to
predict the future state of the system with some level of
confidence. Many approaches use a list of objects and
corresponding tracks and velocities which are considered a
priori known or are learned in an off-line phase. The major
drawback of these methods is that they are suitable only
in industrial controlled environments, where a deterministic
and complete knowledge on other agents is available. A
second class of methods rely on an on-line estimation of
the position and velocity of each object. These methods
lie in general on a multi-target tracking algorithm and a
data association technique which can encounter problems in
cluttered environments and do not face the uncertainty due
to the unobserved space. The advantage of considering a
dynamic occupancy grid is that the robot maintains a full
probabilistic information about the present occupation of the
space and an estimation of the velocity of each occupied
cell in the s patial grid. The absence of high level models
makes the robot able to cope with unexpected situations and
previously unknown obstacles. Furthermore, observations
coming from different sensors can be directly integrated into
the grid, so that the method is easy adaptable to different
mobile bases.
The cell-to-cell approach to the linear velocity obstacles
allows to reduce the hypothesis of the method, taking into
consideration robot and obstacles of whatever shape and
whatever discretized approximation of uncertainty in position
and velocity of the obstacles. In contrast with the worst case
approaches [16], the non observed space contributes directly
to the computation of the probability of collision, leading to
a full probabilistic framework.
III. THE DISCRETE PROBABILISTIC VELOCITY
OBSTACLES
In this section we explain in detail the developed al-
gorithm. Paragraph I II- A describes the generalisation of
velocity obstacles to the cell-to-cell approach. Paragraph
III-B explains how the probability of collision in time is
computed. Paragraph III-C, finally, details how the control
input is chosen for the obstacle avoidance.
A. Cell-to-cell approach
The VO approach explained in the previous section is
a geometric method that determines if a linear velocity
leads the robot to a collision in the future. In order to
generalise the method for a probabilistic approach and to
the input provided by an occupancy grid, we developed a
cell-to-cell approach. In this paragraph we make reference
to a deterministic representation: the occupation of cells
considered is P (Occ) = {0, 1} and the velocity is a priori
known: P
c
(v
o
) = 1, P
c
(v
n
) = 0 ∀n 6= o. The grid is relative
to the robot.
Lets consider the robot and the obstacles as clusters of
occupied cells. The velocities we study for the robot are
integer linear velocities v
n
= [i ·
q
τ
, j ·
q
τ
] where i, j ∈ N
. The search space is reduced t o the velocities reachable
within the next time step: dynamic and kinematic constraints
as a maximum acceleration value and a maximum and
minimum velocity in each direction are specified. Following
the fr amework detailed in the previous section, a velocity
(∆i
r
, ∆j
r
) leads the robot to a collision if it belongs to
at least one of the V O
p
r
c
o
between one of the points of
the robot p
r
and an occupied cell of the grid c
o
. Given
[∆i
o
, ∆j
o
] the velocity of an obstacle in the space, and
[∆
i
, ∆
j
] an admissible velocity of the robot, each relative
velocity (∆
′
i, ∆
′
j) = (∆i − ∆i
o
, ∆j − ∆j
o
) is considered.
Reasoning in the velocity space, this velocity corresponds to
the vector attached to p
r
= [x
r
, y
r
] pointing [x
r
+ ∆
′
i, y
r
+
∆
′
j]. As shown in Fig. 3(a), this velocity belongs to the
VO relative to p
r
iff there is at least an occupied cell with
velocity (∆i
o
, ∆j
o
) in the positive direction of the extension
of the velocity vector.
(a) (b)
Fig. 3. Grey cells are searched for obstacles; black cells are occupied.
A point of the robot and a cell are respectively considered in (a) and (b)
image. The arrow is the considered relative velocity while the red dotted
lines delimit searching areas f or different times to collision.
To consider all the points in the robot-cell centred in
(x
r
, y
r
) we have to check all the cells which fall (also
partially) between the two parallel lines tangent to the cells
centred respectively in (x
r
, y
r
) and (x
r
+ ∆
′
i, y
r
+ ∆
′
j )
in the positive direction. This region is the search obstacle
region of velocity v
′
r
relative to cell r and we denote it
SO(v
′
r
, r). To compute the probability of collision of the
relative velocity at a given time instant t in the future, we
consider the set SO
t
(v
′
r
, r) that should be traversed by the
robot in the interval [t − 1, t]. The considered velocity is in
collision if for at least one of the cell of the robot it is found
one occupied cell with velocity (∆i
o
, ∆j
o
) . It is possible to
introduce some simplification. For each velocity v
r
:
• It is s ufficient to consider just the 4-connected cells on
the contour of the robot, where collisions occur first.
• We can consider just the last contour cells in the velocity
direction.
For what concerns the number of velocities to study, the
search space is reduced to the velocities that can be reached
within the next time step. This dynamic window is centred
around the actual velocity of the robot and is limited by
the maximum acceleration that the motors can exert and
eventually the maximum allowed velocity in function of the
direction.
B. Compute the probability of collision
As detailed in Section II-A, the environment is represented
by a dynamic occupancy grid. Each cell stores a probabilistic
estimation of its state:
• a value of probability of occupation P (Occ) ;
• a probabilistic distribution function on a histogram of
possible velocities P (v
n
), n = 1...N ;
• an index k = −1, 0, . . . , K + 1 , where K is the
estimated number of obstacles in the space (clusters of
the grid); cells considered as free are given −1 index,
cells occluded or not reached by the sensor range are
given index 0; cells indicating the robot are given index
K + 1.
Given a robot velocity v
r
and an obstacle velocity v
n
, the
probability of collision of a cell r with a cell o in the
SO(v
r
′
, r) is:
P
coll
(v
r
, v
n
, r) = P
o
(Occ) · P
o
(v
n
) (5)
as P
r
(Occ) = 1. Considering the whole robot dimension,
the maximum probability of collision in the interval [t−1, t]
is kept for each object k :
P
coll
(v
r
, k, v
n
) = max
o∈O
P
o
(Occ) · P
o
(v
n
) · δ(k
o
) (6)
where o is each cell in SO
t
(v
r
′
, r) and δ(k
o
) = 1 if k
o
= k,
0 otherwise.
To compute the probability of collision P
coll
(v
r
) of the
absolute velocity at time instant t, all the possible velocities
of obstacles have to be considered. This value is computed
as follows: for collisions with the same obstacle k the
probability is given by the sum of the probability of each
velocity, as P (v
i
|v
j
) = 0 for each i, j = 1...N and i 6= j:
P
coll(k )
(v
r
) = P
coll(k )
(v
r
, v
1
) ∨ . . . ∨ P
coll(k )
(v
r
, v
N
)
=
X
n=1...N
P
coll(k )
(v
r
, v
n
) (7)
If the collisions considered are due to different obstacles, the
total probability is given by:
P
coll
(v
r
) = 1 −
X
k=1...K
(1 − P
coll(k )
(v
r
)) (8)
Both equations 7 and 8 are presented and used in the PVO
approach [1].
A cumulative probability of collision from time 0 to the
time step t under investigation is recursively computed.
Applying a velocity v
r
from present to t leads to a collision
if there is a collision in the interval [0, t − 1] or if there is a
collision at time instant t:
P
0...t
(v
r
) = P
0...t−1
(v
r
) + (1 − P
0..t−1
(v
r
)) × P
t
(v
r
) (9)
with the hypothesis that P
coll,0
= 0.
Fig. 4 shows how the computation of the probability of
collision in time reflects the uncertain information about the
environment. We simulated the input that could be provided
by a distance sensor as a laser range finder or a radar. A
maximum range of 20·q is considered. Since we are working
with a probabilistic representation, each cell has in general a
positive probability of occupation: the free space scanned by
the sensor is characterised by a probability of occupation that
is nearly 0 while not sensed environment has P (Occ) = 0.5.
