# 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.

**0**Bookmarks

**·**

**72**Views

- [Show abstract] [Hide abstract]

**ABSTRACT:**This paper presents the field testing results of an autonomous manipulator-based robotic sys-tem that strips the paint and rust from steel bridges [Liu et al., 2008]. The key components of this system are sensing and planning, which have been presented in other research papers. The grit-blasting field trial presented in this pa-per spanned 6 weeks, and included 20 hours over 4.5 days of actual grit-blasting operation. The field testing has verified the algorithms de-veloped for exploration, mapping, surface seg-mentation, robot motion planning and collision avoidance. It has also proved that the robotic system is able to perform bridge maintenance operations (grit-blasting), reduce human work-ers' exposure to hazardous and dangerous de-bris (containing rust, lead-based paint parti-cles), and relieve workers from labour-intensive tasks. The system has been shown to posi-tion a grit-blast nozzle so as to remove the paint and rust at the same rate that is expected of a worker with equivalent equipment: small grit-blasting pot and medium-sized hose noz-zle. Testing in the field has also highlighted important issues that need to be addressed. - SourceAvailable from: Santiago Garrido
##### Conference Paper: Outdoor Motion Planning Using Fast Marching

CLAWAR 2009, Istambul. Turkey; 09/2009 - 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. e-mail: sgarrido@ing.uc3m.es

• Prof. Luis Moreno, Robotics Lab., System Engineering and Automation Dept., Carlos III University, Madrid,

Spain. e-mail: moreno@ing.uc3m.es

• Prof. Dolores Blanco, Robotics Lab., System Engineering and Automation Dept., Carlos III University, Madrid,

Spain. e-mail: 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. Sensor-based 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. Sensor-based 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 grid-based 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

grid-based 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 In-Motion

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: follow-the-mid-line or follow-the-left-wall. 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 office-like 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 wall-following combined with obstacle avoidance. Oriolo

et al. [14] developed a grid-based environment mapping process that uses fuzzy logic to update the grid cells.

The mapping process runs on-line [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 re-plans 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 grid-based 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 feature-based 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 Frontier-Based 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 two-dimensional 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 sensor-based 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 Rimon-Koditscheck [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 path-planning 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 nearest-neighbor search on an optimized

kd-tree, 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 linear-time

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:

|∇T|F = 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 H-shaped 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 H-shaped 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 real-valued 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 real-valued, 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))

|∇u|2=ω2

c2

Surfaces of constant u describe the wave fronts. In the Sethian [30] notation

|∇T|F = 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