Exploration of a cluttered environment using Voronoi Transform and Fast Marching.
ABSTRACT The Extended Voronoi Transform and the Fast Marching Method combination provide potential maps for robot navigation in previously unexplored dynamic environments. The Extended Voronoi Transform of a binary image of the environment gives a grey scale that is darker near the obstacles and walls and lighter far from them. The Logarithm of the Extended Voronoi Transform imitates the repulsive electric potential from walls and obstacles. The method proposed, called Voronoi Fast Marching method, uses a Fast Marching technique on the Extended Voronoi Transform of the environment’s image, provided by sensors, to determine a motion plan. The computational efficiency of the method lets the planner operate at high rate sensor frequencies. This avoids the need for collision avoidance algorithms. The robot is directed towards the most unexplored and free zones of the environment so as to be able to explore all the workspace. This method is very fast and reliable and the trajectories are similar to the human trajectories: smooth and not very close to obstacles and walls. In this article we propose its application to the task of exploring unknown environments.

Conference Paper: Robot Formations Motion Planning using Fast Marching
ROBOT: Robótica Experimental; 01/2012  SourceAvailable from: Santiago Garrido
Conference Paper: How to deal with difficulty and uncertainty in the Outdoor Trajectory Planning with Fast Marching
[Show abstract] [Hide abstract]
ABSTRACT: This paper presents an interesting technique for �nding the trajectory of an outdoor robot. This technique applies Fast Marching to a 3D surface terrain represented by a triangular mesh in order to calculate a smooth trajectory between two points. The method uses a triangular mesh instead of a square one because this kind of grid adapts better to 3D surfaces. The novelty of this approach is that, in the �rst step of the method, the algorithm calculates a weight matrix W that can represents di�culty, viscosity, refraction index or incertitude based on the informa tion extracted from the 3D surface characteristics and the sensor data of the robot. Within the bestowed experiments these features are the height, the spherical variance, the gradient of the surface and the incer titude in the position of other objects or robots and also the incertitude in the map because some portions of the map can't be measured directly by the robot. This matrix is used to limit the propagation speed of the Fast Marching wave in order to �nd the best path depending on the task requirements, e.g., the trajectory with least energy consumption, the fastest path, the most plain terrain or the safest path. The method also gives the robot's maximum admisible speed, which depends on the wave front propagation velocity. The results presented in this paper show that it is possible to model the path characteristics as desired, by varying this matrix W. Moreover, as it is shown in the experimental part, this method is also useful for calculating paths for climbing robots in com plex purely 3D environments. At the end of the paper, it is shown that this method can also be used for robot avoidance when two robots with opposite trajectories approach each other, knowing each others position.Iberian Robotics Conference (ROBOT2013), Madrid, Spain; 11/2013  SourceAvailable from: Santiago Garrido
Conference Paper: Improving RRT motion trajectories using VFM
5th IEEE International Conference on Mechatronics (ICM 2009), Malaga, Spain; 04/2009
Page 1
1
CATEGORIES: (5)
Title:
ExplorationofaClutteredEnvironmentusingVoronoiTransformandFastMarchingMethod
Authors:
• Prof. Santiago Garrido, Robotics Lab., System Engineering and Automation Dept., Carlos III University,
Madrid, Spain. email: sgarrido@ing.uc3m.es
• Prof. Luis Moreno, Robotics Lab., System Engineering and Automation Dept., Carlos III University, Madrid,
Spain. email: moreno@ing.uc3m.es
• Prof. Dolores Blanco, Robotics Lab., System Engineering and Automation Dept., Carlos III University, Madrid,
Spain. email: dblanco@ing.uc3m.es
Abstract
The Extended Voronoi Transform and the Fast Marching Method combination provide potential maps for robot
navigation in previously unexplored dynamic environments. The Extended Voronoi Transform of a binary image of
the environment gives a grey scale that is darker near obstacles and walls and lighter when far from them. The
Logarithm of the Extended Voronoi Transform imitates the repulsive electric potential from walls and obstacles.
The method proposed, called the Voronoi Fast Marching method, uses a Fast Marching technique on the Extended
Voronoi Transform of the environment’s image provided by sensors to determine a motion plan. The computational
efficiency of the method lets the planner operate at high rate sensor frequencies. This avoids the need for collision
avoidance algorithms. The robot is directed towards the most unexplored and free zones of the environment so as
to be able to explore all the workspace. This method is very fast and reliable and the trajectories are similar to the
human trajectories: smooth and not very close to obstacles and walls. In this article we propose its application in
the exploration task of unknown environments.
Keywords:
Navigation map, mobile robots, global localization, evolutive algorithm, robot mapping.
Page 2
2
I. INTRODUCTION
Sensor based exploration is a fundamental function for mobile robot intelligence. There is a variety of potential
applications for autonomous mobile robots in such diverse areas as forestry, space, nuclear reactors, environmental
disasters, industry, and offices. Tasks in these environments are often hazardous to humans, in a remote located area,
or tedious to perform. Potential tasks for autonomous mobile robots include maintenance, delivery, and security
surveillance, which all require some form of intelligent navigational capabilities. A mobile robot is a useful addition
to these domains only when it is capable of [30] functioning robustly under a wide variety of environmental
conditions, [20] operating without human intervention for long periods of time, and [18] providing some guarantee
of task performance. The environments in which mobile robots must function are dynamic, unpredictable and not
completely specifiable by a map beforehand. In order for the robot to successfully complete a set of tasks, it must
dynamically adapt to changing environmental circumstances. Sensorbased discovery path planning is the guidance
of an agent  a robot  without a complete a priori map, by discovering and negotiating the environment so as to
reach a goal location while avoiding all encountered obstacles. Sensorbased discovery (i.e., dynamic) path planning
is problematic because the path needs to be continually recomputed as new information is discovered.
In order to explore an unknown environment, this paper presents a exploration and path planning method based
on the Logarithm of the Extended Voronoi Transform and the Fast Marching Method. In each step of the exploration
process the sensors provide a binary image of the visible environment having distinguished the detected obstacles
of the free space. The Extended Voronoi Transform of an image gives a grey scale that is darker near the obstacles
and walls and lighter when far from them. The Logarithm of the Extended Voronoi Transform imitates the repulsive
electric potential in 2D from walls and obstacles. This potential impels the robot to follow a trajectory far from
obstacles.
The last step is to calculate the trajectory in the image generated by the Logarithm of the Extended Voronoi Trans
form using the Fast Marching Method. Then, the path obtained verifies the smoothness and safety considerations
required for mobile robot path planning.
The Fast Marching Method has been applied to Path Planning [30], and their trajectories are of minimal distance,
but they are not very safe because the path is too close to obstacles and what is more important, the path is not
Page 3
3
smooth enough.
In order to improve the safety of the trajectories calculated by the Fast Marching Method, two solutions are
possible:
The first possibility, in order to avoid unrealistic trajectories produced when the areas are narrower than the robot,
the segments with distances to the obstacles and walls smaller than the size of the robot need to be removed from
the Voronoi diagram previous to the Extended Voronoi Transform.
The second possibility, used in this work, it is to dilate the objects and walls in a security distance that assure
that the robot not collide and does not accept passages narrower than the robot’s size.
The advantages of this method are its easy implementation, its speed and the quality of the trajectories. The
method works in 2D and 3D, and can be used on a local scale operating with sensor information.
II. PREVIOUS AND RELATED WORKS
A. Representations of the world
Roughly speaking there are two main forms for representing the spatial relations in an environment: metric maps
and topological maps. Metric maps are characterized by a representation where the position of the obstacles are
indicated by coordinates in a global frame of reference. Some of them represent the environment with grids of
points, defining regions that can be occupied or not by obstacles or goals [18], [20]. Topological maps represent
the environment with graphs that connect landmarks or places with special features [24] [3]. In our approach we
choose the gridbased map to represent the environment. The clear advantage is that with grids we already have
a discrete environment representation and ready to be used in conjunction with the Extended Voronoi Transform
function and Fast Marching Method for path planning. The pioneer method for environment representation in a
gridbased model was the certainty grid method developed at Carnegie Mellon University [18] by Moravec. He
represents the environment as a 3D or 2D array of cells. Each cell stores the probability of the related region
being occupied. The uncertainty related to the position of objects is described in the grid as a spatial distribution
of these probabilities within the occupancy grid. The larger the spatial uncertainty, the greater the number of cells
occupied by the observed object. The update of these cells is performed during the navigation of the robot or
Page 4
4
through the exploration process by using an update rule function. Many researchers have proposed their own grid
based methods. The main difference among them is the function used to update the cell. Some of them are, for
example: Fuzzy [13], Bayesian [19], Heuristic Probability [10], Gaussian [11], etc. In the Histogramic InMotion
Mapping (HIMM), each cell, has a certainty value, which is updated whenever it is being observed by the robots
sensors. The update is performed by increasing the certainty value by 3 (in the case of detection of an object) or
by decreasing it by 1 (when no object is detected), where the certainty value is an integer between 0 and 15.
B. Approaches to exploration
This section relates some interesting techniques used for exploratory mapping. They mix different localization
methods, data structures, search strategies and map representations. Kuipers and Byun [4] proposed an approach to
explore an environment and to represent it in a structure based on layers called Spatial Semantic Hierarchy (SSH)
[3]. The algorithm defines distinctive places and paths, which are linked to form an environmental topological
description. After this, a geometrical description is extracted. The traditional approaches focus on geometric
description before the topological one. The distinctive places are defined by their properties and the distinctive
paths are defined by the twofold robot control strategy: followthemidline or followtheleftwall. The algorithm
uses a lookup table to keep information about the place visited and the direction taken. This allows a search
in the environment for unvisited places. Lee [23] developed an approach based on Kuipers work [4] on a real
robot. This approach is successfully tested in indoor officelike spaces. This environment is relatively static during
the mapping process. Lee’s approach assumes that walls are parallel or perpendicular to each other. Furthermore,
the system operates in a very simple environment comprised of cardboard barriers. Mataric [24] proposed a map
learning method based on a subsumption architecture. Her approach models the world as a graph, where the
nodes correspond to landmarks and the edges indicate topological adjacencies. The landmarks are detected from
the robot movement. The basic exploration process is wallfollowing combined with obstacle avoidance. Oriolo
et al. [14] developed a gridbased environment mapping process that uses fuzzy logic to update the grid cells.
The mapping process runs online [13], and the local maps are built from the data obtained by the sensors and
integrated into the environment map as the robot travels along the path defined by the A∗algorithm to the goal.
The algorithm has two phases. The first one is the perception phase. The robot acquires data from the sensors and
Page 5
5
updates its environment map. The second phase is the planning phase. The planning module replans a new safe
path to the goal from the new explored area. Thrun and Bucken [28][29] developed an exploration system which
integrates both evidence grids and topological maps. The integration of the two approaches has the advantage of
disambiguating different positions through the gridbased representation and performing fast planning through the
topological representation. The exploration process is performed through the identification and generation of the
shortest paths between unoccupied regions and the robot. This approach works well in dynamic environments,
although, the walls have to be flat and cannot form angles that differ more than 15◦from the perpendicular. Feder
et al. [17] proposed a probabilistic approach to treat the concurrent mapping and localization using a sonar. This
approach is an example of a featurebased approach. It uses the extended Kalman filter to estimate the localization
of the robot. The essence of this approach is to take actions that maximize the total knowledge about the system in
the presence of measurement and navigational uncertainties. This approach was tested successfully in wheeled land
robot and autonomous underwater vehicles (AUVs). Yamauchi [35][5] developed the FrontierBased Exploration
to build maps based on grids. This method uses a concept of frontier, which consists of boundaries that separate
the explored free space from the unexplored space. When a frontier is explored, the algorithm detects the nearest
unexplored frontier and attempts to navigate towards it by planning an obstacle free path. The planner uses a depth
first search on the grid to reach that frontier. This process continues until all the frontiers are explored. Zelek [37]
proposed a hybrid method that combines a local planner based on a harmonic function calculation in a restricted
window with a global planning module that performs a search in a graph representation of the environment created
from a CAD map. The harmonic function module is employed to generate the best path given the local conditions
of the environment. The goal is projected by the global planner in the local windows to direct the robot. Recently,
Prestes el al. [33] have investigated the performance of an algorithm for exploration based on partial updates of a
harmonic potential in an occupancy grid. They consider that while the robot moves, it carries along an activation
window whose size is of the order of the sensors range.
Prestes and coworkers [34] propose an architecture for an autonomous mobile agent that explores while mapping
a twodimensional environment. The map is a discretized model for the localization of obstacles, on top of which
a harmonic potential field is computed. The potential field serves as a fundamental link between the modeled
Page 6
6
(discrete) space and the real (continuous) space where the agent operates.
The proposed method in this paper can be included in the sensorbased global planner paradigm. It is a potential
method but it does not have the typical problems of these methods enumerated by Koren Borenstein [3]: 1) Trap
situations due to local minima (cyclic behavior). 2) No passage between closely spaced obstacles. 3) Oscillations
in the presence of obstacles. 4) Oscillations in narrow passages. The proposed method is conceptually close to the
navigation functions of RimonKoditscheck [6], because the potential field has only one local minimum located
at the single goal point. This potential and the paths are smooth (the same as the repulsive potential function)
and there are no degenerate critical points in the field. These properties are similar to the characteristics of the
electromagnetic waves propagation in Geometrical Optics (for monochromatic waves with the approximation that
length wave is much smaller than obstacles and without considering reflections nor diffractions).
The Fast Marching Method has been used previously in Path Planning by Sethian [32], [31], but using only an
attractive potential. This method has some problems. The most important one that typically arises in mobile robotics
is that optimal motion plans may bring robots too close to obstacles (including people), which is not safe. This
problem has been dealt with by Latombe [22], and the resulting navigation function is called NF2. The Voronoi
Method also tries to follow a maximum clearance map [16]. Melchior, Poty and Oustaloup [26], [1], present a
fractional potential to diminish the obstacle danger level and improve the smoothness of the trajectories, Philippsen
[27] introduces an interpolated Navigation Function, but with trajectories too close to obstacles and without smooth
properties and Petres [7], introduces efficient pathplanning algorithms for Underwater Vehicles taking advantage
of the underwaters currents.
To achive a smooth and safe path,it is necessary to have smooth attractive and repulsive potentials, connected in
such a way that the resulting potential and the trajectories have no local minima and curvature continuity to facilitate
path tracking design. The main improvement of the proposed method are these good properties of smoothness and
safety of the trajectory. Moreover, the associated vector field allows the introduction of nonholonomic constraints.
It is important to note that in the proposed method the important ingredients are the attractive and the repulsive
potentials, the way of connecting them describing the attractive potential using the wave equation (or in a simplified
way, the eikonal equation). This equation can be solved in other ways: Mauch[25] uses a Marching with Correctness
Page 7
7
Criterion with a computational complexity that can reduced to O(N). Covello[8] presents a method that can be
used on nodes that are located on highly distorted grids or on nodes that are randomly located.
III. INTRODUCTION TO THE EXTENDED VORONOI TRANSFORM
The Distance Transform[2] is a useful tool in digital picture processing. It has found a wide range of uses in
image analysis, pattern recognition, and robotics. In Computer Vision, it is known as Distance Transform, but this
term is also used in Robotics to designate a different concept. For this reason, in Robotics, this concept is called
Extended Voronoi Transform.
The Extended Voronoi Transform computes the Euclidean Distance of the binary image. For each pixel in the
image, the Extended Voronoi Transform assigns a number which is the distance to the nearest nonzero pixel of
the image. In a binary image, a pixel is referred to as background if its value is zero. For a given distance metric,
the Extended Voronoi Transform of an image produces a distance map of the same size. For each pixel inside the
objects in the binary image, the corresponding pixel in the distance map has a value equal to the minimum distance
to the background.
Clearly, the Extended Voronoi Transform is closely related to the Voronoi diagram. The Voronoi diagram concept
is involved in many Extended Voronoi Transform approaches either explicitly or implicitly.
For any topologically discrete set S of points in Euclidean space and for almost any point x, there is one point
of S to which x is closer than x is to any other point of S. The word "almost" is occasioned by the fact that a
point x may be equally close to two or more points of S. If S contains only two points, a, and b, then the set of
all points equidistant from a and b is a hyperplane, i.e. an affine subspace of codimension 1. That hyperplane is
the boundary between the set of all points closer to a than to b, and the set of all points closer to b than to a.
In general, the set of all points closer to a point c of S than to any other point of S is the interior of a convex
polytope (in some cases unbounded) called the Dirichlet domain or Voronoi cell for c. The set of such polytopes
tesselates the whole space, and is the Voronoi tessellation corresponding to the set S. If the dimension of the
space is only 2, then it is easy to draw pictures of Voronoi tessellations, and in that case they are sometimes called
Voronoi diagrams.
Page 8
8
a)b)c)d)
Fig. 1.Propagation of a wave and the corresponding minimum time path when there are two media of different slowness (diffraction)
index. (a), the same with an vertical gradient (b), Wavefront propagating with velocity F (c), and Formulation of the arrival function T(x),
for an unidimensional wavefront.(d).
The close relation between EVT and the Voronoi Diagram implies that it is possible to compute one of them
from the other. The majority of the EVT methods use the Voronoi Diagram as an intermediate step.
For more than two dimensions, the Extended Voronoi Transform uses a nearestneighbor search on an optimized
kdtree, as described by Friedman[12].
The algorithm used in this work for two dimensions, is based on the second algorithm work carried out by Breu
et al. [6]. In this work, the special properties of the Euclidean metric are exploited. They designed two lineartime
algorithms based on Voronoi transforms where the second algorithm could have been improved if they had used
the result of the previous row to reduce the set of possible candidates. It is an O(m × n) algorithm, where the
image size is m × n.
The Voronoi approach to path planning has the advantage of providing the safest trajectories in terms of distance
to obstacles but because its nature is purely geometric and it does not achieve enough smoothness.
IV. INTUITIVE INTRODUCTION OF THE EIKONAL EQUATION AND THE FAST MARCHING PLANNING METHOD
Intuitively, Fast Marching Method gives the propagation of a front wave in an inhomogeneous media as shown
in fig 1a and b.
Let us imagine that the curve or surface moves in its normal direction with a known speed F(see fig. 1c). The
objective would be to follow the movement of the interface while this one evolves. A large part of the challenge,
in the problems modeled as fronts in evolution, consists in defining a suitable speed, which faithfully represents
Page 9
9
Fig. 2.Movement of a circular wavefront, as a problem of boundary conditions
the physical system.
A way to characterize the position of a front in expansion is to compute the time of arrival T, in which the
front reaches each point of the underlying mathematical space of the interface. It is evident that for one dimension
(see fig. 1d) the equation for the arrival function T can be obtained in an easy way, simply by considering the fact
that the distance x is the product of the speed F by the time T: x = F ∙ T. The spatial derivative of the solution
function becomes the gradient: 1 = FdT
dxand therefore we have that the magnitude of the gradient of the arrival
function T(x) is inversely proportional to the speed,
1
F= ∇T . For multiple dimensions, the same concept is
valid because the gradient is orthogonal to the level sets of the arrival function T(x). In this way, the movement
of the front can be characterized as the solution of a boundary conditions problem. The speed F depends only on
the position, then the equation
1
F= ∇T or the Eikonal equation:
∇TF = 1.
(1)
As a simple example we define a circular front γt= {(x,y)/T(x,y) = t} for two dimensions that advance with
unitary speed. The evolution of the value of the arrival function T(θ) can be seen as the time increases (i.e. T = 0,
T = 1, T = 2, ...) and the arrival function comes to points of the plane in more external regions of the surface as
can be seen in fig. 2. The boundary condition is that the value of the wave front is zero in the initial curve.
The direct use of the Fast Marching method does not guarantee a smooth and safe trajectory. Due to the way the
front wave is propagated the shortest geometrical path is determined. This makes the trajectory unsafe because it
touches corners, walls and obstacles, as is shown in figure 5. This problem can be easily solved by enlarging the
Page 10
10
a)b)
Fig. 3. Union of the two potentials: the second one having the first one as refractive index. a) Associated vector field and a typical trajectory
obtained with this method. b) The Funnel shaped potential applied to the first potential of a Hshaped environment and the path calculated
with the gradient method.
obstacles, but even in that case the trajectory tends to get close to the walls and it is not smooth and safe enough.
The use of the Fast Marching method over a slowness (refraction or inverse of velocity) potential improves the
quality of the calculated trajectory considerably. On one hand, the trajectories tend to go close to the Voronoi
skeleton because of the optimal conditions of this area for robot motion[15]. On the other hand, the trajectories are
also considerably smooth. For a small and easy Hshaped environment, the slowness (velocity inverse) potential
in 3D is shown in fig. 3b and the funnel shaped potential given by the wave propagation of and the trajectory
calculated by the gradient method is shown in fig. 3a.
For further details and summaries of level set and fast marching techniques for numerical purposes, see (Sethian
[31]). The Fast Marching Method is an O(n) algorithm as has been demonstrated by Yatziv [36].
V. INTUITIVE INTRODUCTION TO THE VORONOI FAST MARCHING METHOD (VFM)
Which properties and characteristics are desirable for a Motion Planner of a mobile robot? The first one is that
the planner always drives the robot in a smooth and safe way to the goal point. In Nature there are phenomena
with the same way of working: electromagnetic waves. If in the goal point, there is an antenna that emits an
electromagnetic wave, then the robot could drive himself to the destination following the waves to the source. The
concept of the electromagnetic wave is especially interesting because the potential and its associated vector field,
Page 11
11
see figure 3, have all the good properties desired for the trajectory, such as smoothness (it is C∞) and the absence
of local minima.
This attractive potential still has some problems. The most important one that typically arises in mobile robotics
is that optimal motion plans may bring robots too close to obstacles, which is not safe. This problem has been
dealt with by Latombe [22], and the resulting navigation function is called NF2. The Voronoi Method also tries to
follow a maximum clearance map [16]. To get a safe path, it is necessary to add a component that repels the robot
away from obstacles. In addition, this repulsive potential and its associated vector field should have good properties
such those of electrical field. If we consider that the robot has an electrical charge of the same sign as obstacles,
then the robot would be pushed away from obstacles. The properties of this electric field are very good because it
is smooth and there are no singular points in the interest space (Cfree).
The third part of the problem consists in how to mix the two fields together. This union between an attractive and
a repulsive fields has been the biggest problem for the potential fields in path planning since the works of Khatib
[21]. In our exposition, this problem has been solved in the same way that Nature does so: the electromagnetic
waves, as light, have a propagation velocity that depends on the media. For example flint glass has a refraction
index of 1.6, while in the air it is approximately one. This refraction index of a medium is the quotient between
the velocity of light in the vacuum and the velocity in the medium. That is the slowness index of the front wave
propagation of a medium.
For this reason, in the VFM proposed method, the repulsive potential is used as refraction index of the wave
emitted from the goal point. This way a unique field is obtained and its associated vector field is attractive to
the goal point and repulsive from the obstacles. This method inherits the properties of the electromagnetic field.
Intuitively, the VFM Method gives the propagation of a front wave in an inhomogeneous media as shown in fig 1a
and b.
VI. ALMOST FLAT WAVE APPROACH OF THE WAVE EQUATION: THE EIKONAL EQUATION
Maxwell’s laws govern the propagation of the electromagnetic waves and can be modeled with the wave equation:
∂2φ
∂t2= c2∇2φ
Page 12
12
In electrodynamics, each component of the fields satisfies the wave equation where c2=
c2
μ?. (c0is the speed of
0
light in a vacuum, μ is the permeability and ? is the dielectric constant). A solution of the wave equation is called
a wave. The moving boundary of a disturbance is called a wave front. In this section we will show how the wave
front can be described by an eikonal equation. We follow the reasoning presented in [9]. If c is constant, then there
are plane traveling wave solutions of the form
φ = φ0ei(kx−ωt)
(We can take the real or imaginary part to obtain a realvalued solution). Here the constant φ0is the amplitude and
ω is the frequency. The wave number vector k is in the direction of the wave. This is perpendicular to the wave
fronts which satisfy kx − ωt = constant. The wave number k is the length of the wave vector, k =
√k ∙ k and
satisfies k =ω
c. The index of refraction η is defined by c =c0
η. Let k0be the wave number in a vacuum where the
index of refraction is unity. For simplicity, consider a wave propagating in the first coordinate direction.
φ = φ0eik0(ηx−c0t)
(2)
Here we have factorized out k0because we will be considering the case where the wave number is large. Now
we consider the case that the index of refraction η is spatially dependent. We seek a solution that is similar to the
plane wave in eq. 2.
φ = exp(A(x) + ik0(ψ(x) − c0t))
(3)
Here the amplitude eAand the phase k0ψ are determined by the slowly varying functions A(x) and ψ(x).
To study the transmission of rays, a useful approach is the "almost flat waves" (i.e. the optical wave propagates
at wavelengths much smaller than image objects so that ray optics can approximate wave optics) in isotropic and
possibly non homogeneous media for a monochromatic wave. These equations along with the mentioned approach,
allow us to develop the theories based on rays such as: geometrical optics, the theory of sound waves, etc. We
compute the derivatives of this approximate almost flat wave.
∇φ = φ∇(A + ik0ψ)
Page 13
13
∇2φ = φ(∇2A + ik0∇2ψ + (∇A)2− k2
0(∇ψ)2+ i2k0∇A ∙ ∇ψ)
We substitute eq. 3 into the wave equation.
η2
c2
0
= ∇2φ
−k2
0η2= ∇2A + ik0∇2ψ + (∇A)2− k2
0(∇ψ)2+ i2k0∇A ∙ ∇ψ
Since A and ψ are realvalued, we equate the real and imaginary parts.
∇2A + (∇A)2+ k0(η2− (∇ψ)2) = 0
2∇A ∙ ∇ψ + ∇2ψ = 0
We assume that η varies slowly on the length scale of a wavelength, λ = 2π/k. Alternatively, for a fixed function
η, we assume that the frequency is high (the wavelength is short). This is the geometrical optics approximation.
For large k0, the first equation is approximately solved by an eikonal equation:
∇ψ2= η2
We rewrite this eikonal equation in terms of the phase u of the wave.
φ = exp(A(x) + i(u(x) − ωt))
∇u2=ω2
c2
Surfaces of constant u describe the wave fronts. In the Sethian [30] notation
∇TF = 1
where T(x) represents the wavefront (time), F(x) is the slowness index of the medium.
In Geometrical Optics, Fermat’s least time principle for light propagation in a medium with space varying
refractive index η(x) is equivalent to the eikonal equation and can be written as ∇Φ(x) = η(x) where the
eikonal Φ(x) is a scalar function whose isolevel contours are normal to the light rays. This equation is also known
as Fundamental Equation of the Geometrical Optics.
The eikonal (from the Greek "eikon", which means "image") is the phase function in a situation for which the
phase and amplitude are slowly varying functions of position. Constant values of the eikonal represent surfaces of
Page 14
14
constant phase, or wavefronts. The normals to these surfaces are rays (the paths of energy flux). Thus the eikonal
equation provides a method for "ray tracing" in a medium of slowly varying index of refractive (or the equivalent
for other kinds of waves).
VII. DETAILS OF THE VFM ALGORITHM
This method starts with the calculation of the Logarithm of the Extended Voronoi Transform of the 2D a priori
map of the environment (or the Extended Voronoi Transform in case of 3D maps). Each white point of the initial
image (which represents free cells in the map) is associated with a level of grey that is the logarithm of the 2D
distance to nearest obstacles (or the Extended Voronoi Transform in 3D). As a result of this process, a kind of
potential proportional to the distance to the nearest obstacles to each cell is obtained, see figure 4. Zero potential
indicates that a given cell is part of an obstacle and maxima potential cells corresponds to cells located in the
Voronoi diagrams (which are the cells located equidistant to the obstacles).
This function introduces a potential similar to a repulsive electric potential (in 2D) as shown in figure 4, that
can be expressed by
φ = c1log(r) + c2.
(4)
where c1is a negative constant.
If n > 2 , (n is the space dimension), the potential is
φ =
c3
rn−1+ c4.
(5)
where r is the distance from the origin.
In a second step, the technique proposed here uses Fast Marching to calculate the shortest trajectory in the
potential surface defined by logarithm of the Extended Voronoi Transform. The calculated trajectory is the geodesic
one in the potential surface, i.e. with a viscous distance. This viscosity is done by the grey level. If the Fast
Marching Method were used directly on the environment map, we would obtain the shortest geometrical trajectory,
as shown in fig. 5, but the trajectory is not safe nor smooth.
The potential created has local minima as shown in fig. 6 and 7, but the trajectories are not stuck in these points
because the Fast Marching Method gives the trajectories that correspond to the propagation of a wave front which
Page 15
15
Fig. 4. Potential of the Logarithm of the inverse of Extended Voronoi Transform.
Fig. 5. Trajectory calculated with Fast Marching without the Logarithm Extended Voronoi Transform.
is faster in lighter regions and slower in the darker ones.
The trajectories obtained by using the logarithm of the EVT tend to go by the Voronoi diagram but properly
smoothed as shown figure 6.
VIII. PROPERTIES
The proposed V FM algorithm has the following key properties:
• Fast response. The planner needs to be fast enough to be used reactively in case unexpected obstacles make it
necessary to plan a new trajectory. To obtain this fast response, a fast planning algorithm and fast and simple
treatment of the sensor information is necessary. This requires a low complexity order algorithm for a real
time response to unexpected situations. As shown in table I, the proposed algorithm has a fast response time