Contents lists available at SciVerse ScienceDirect
Robotics and Autonomous Systems
journal homepage: www.elsevier.com/locate/robot
Application of the fast marching method for outdoor motion planning in roboticsSantiago Garrido ∗, María Malfaz, Dolores BlancoRobotics Lab., Carlos III University of Madrid, Av de la Universidad 30, Leganés, Madrid, Spain
a r t i c l e i n f o
Article history:Received 24 May 2012Received in revised form9 October 2012Accepted 15 October 2012Available online 17 November 2012
In this paper, a new path planning method for robots used in outdoor environments is presented. Theproposed method applies Fast Marching to a 3D surface represented by a triangular mesh to calculate asmooth trajectory from one point to another. The method uses a triangular mesh instead of a square onesince this kind of grid adapts better to 3D surfaces. The novelty of this approach is that, before runningthe algorithm, the method calculates a weight matrixW based on the information extracted from the 3Dsurface characteristics. In the presented experiments these features are the height, the spherical variance,and the gradient of the surface. This matrix can be viewed as a difficulty map situated over the 3D surfaceand is used to limit the propagation speed of the Fast Marching wave in order to find the best pathdepending on the task requirements, e.g., the least energy consumption path, the fastest path, or themost plain terrain. The algorithm also gives the speed for the robot, which depends on the wave frontpropagation speed. The results presented in this paper show how, by varying this matrix W , the pathsobtained are different. Moreover, as it is shown in the experimental part, this algorithm is also usefulfor calculating paths for climbing robots in much more complex environments. Finally, at the end of thepaper, it is shown that this algorithm can also be used for robot avoidance when two robots approacheach other, and they know each other’s position.
The objective of a path planner for a mobile robot operatingin environments with unknown obstacles (dynamic or not), is tocalculate collision-free trajectories.
The research carried out in this paper has mainly beeninspired by the project Robauco [1], whose main objective is thedevelopment of technologies for obtaining mobile robots capableof complex tasks that demand a high degree of autonomy andcapacity of collaboration in the presence of human beings. Oneof the main applications of our work is the use of mobile robotsin dangerous missions where the environment can be risky forhumans (e.g., rescue missions, etc.). In this case, an unmannedautonomous vehicle (UAV) is sent in advance to obtain imagesof the outdoor environment. Using the motion planning methodproposed, the best trajectory to reach the goal is obtained overthese images, and this trajectory is sent to the mobile robot.Since the UAV takes images consecutively, in case of changes in theenvironment, the path can be recalculated using the samemethod.
Fast Marching (FM) techniques have been used directly inPath Planning methods to calculate trajectories from one pointto another. Nevertheless, following this strategy, the generated
trajectories are not guaranteed to be safe and smooth. As explainedin [2], the shortest geometrical path obtained can be unsafe, sinceit can touch the corners, walls, and obstacles, as shown in Fig. 1.
Moreover, in most works, these methods are used in 2D en-vironments (indoor environments), whilst the method presentedhere adds another dimension to the problem (outdoor environ-ments).
The new method proposed in this paper consists of severalphases. First, as previously introduced, a 2 1
2 or 3D image of thesurface is needed. Then, a triangular mesh is constructed overthe image, which allows us to generate realistic surfaces due tothe capabilities of the triangles to fit the characteristics of themap better. Once the grid is constructed, the method extractssome information from the environment to obtain the height andto calculate the gradient and the spherical variance, which givesinformation about the roughness of the surface. Then, it combinesthese data with the robot limitations to generate a weight matrixW . This matrix can be view as a difficulty or viscosity mapwhich issituated on the 3D surface. Once the matrix is ready, the methodapplies the FM algorithm over this modified surface (the grid+matrix W ) to generate the path.
If matrix W is not used, the trajectory obtained will be just thegeodesic distance, i.e., the length of the shortest path between thetwo points. Applying matrixW , the proposed method gives a pathwhich considers the features of the surface and the limitations ofthe robot. Moreover, it also gives us information about the speedof the robot based on the FM wave propagation speed [2,3].
S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114 107
Fig. 1. Trajectory calculated by the Fast Marching method directly.
The results presented are carried out in the context of the projectRobauco, that is, it is assumed that the images used are taken inadvance by a UAV. First, the algorithm is used to calculate differentpaths, for one robot on the earth, by varying the ponder factors of theW matrix. The fast execution of this method allows the updatingof the given path as the images of the environment are updated.Therefore, this could be considered as an on-line path planningmethod. Secondly, this algorithm is also applied for avoidancecollision in the case of two robots approaching. In both approaches,weprove that, using the proposedmethod, it is possible to generatesmooth and safe plans in outdoor environments.
The remainder of the paper is organized as follows. In Section 2,a brief summary of other works related to grid-based methodsand path planning methods in outdoor environments is presented.Next, Section 3 introduces an explanation about the FM methodand how this algorithm can be implemented on orthogonal andtriangulated meshes. The following section, Section 4, introducesthe viscosity matrix W and how it is formed. Section 5 presentssome results obtained by simulation. Finally, the main conclusionsof this paper are summarized in Section 6.
2. Related work
To solve the motion planning problem it is necessary to makeuse of an appropriate representation of the environment onwhich to implement effective planning algorithms. This sectionpresents a brief review of grid-based planning methods, since theapproach presented in this paper uses that type of environmentrepresentation.
2.1. Grid-based methods
In the navigation of mobile robots, many methods are based ona grid-based representation of the environment. In these methodseach cell has a binary representation (occupied or free) or an asso-ciated weight that represents the difficulty of traversing that area.
This grid is usually approximated by a graph, with the nodessituated in the center of each cell. Many algorithms have beendeveloped to find a path in the graph.
Grid- or graph-basedmethods, such asDijkstra’s orA∗, calculatea navigation function constrained to movement choices alonggraph edges that are neither optimal for execution nor anexact solution. The well-known Dijkstra’s algorithm computesthe optimal path between a single source point to any otherpoint in the graph. This algorithm is indeed efficient, but suffersfrom metrication errors [4]. A∗ uses a heuristic to focus thesearch from a particular start location towards the goal andthus produces a path from a single location to the goal veryefficiently [5]. D∗, Incremental A∗, and D∗ Lite are extensions ofA∗ that incrementally repair solution paths when changes occur
in the underlying graph [6]. These incremental algorithms havebeen used extensively in robotics for mobile robot navigation inunknown or dynamic environments. To reduce the computationtime [7] suboptimal variants of the search algorithms havebeen used, updating existing solutions efficiently when newinformation is received [6,8] to use this type of approach indynamic environments.
The graph-based algorithms consider the image as an orientedgraph in which a pixel is a node, and the 4 (or 8) connections tothe neighboring pixels are the vertices of the graph. Thesemethodscannot converge to the solution of a continuous problem. Even forvery fine grids, the paths restrict the agent’s heading to incrementsof π/2 or π/4. This results in paths that are suboptimal in lengthand difficult to traverse in practice. Frequently, a smoothing step isperformed after planning to alleviate this problem, which yieldsunsatisfactory results because it only locally solves the discretenature of the problem. The different metrics used lead to verydifferent results. Various efforts have been made to overcomethese shortcomings, including increasing the available headings orapproximating continuous paths (see [9,10]).
Our approach addresses this problem using the FM method.The FMmethod uses the L2 metric, instead of the similar methodsof the L1 (Manhattan) of Dijkstra, and it makes an intrinsicinterpolation that allows us to solve efficiently the continuouslight propagation equation (the Eikonal equation) to calculate theshortest path, as shown in Fig. 2. In other words, although the FMmethod uses data obtained from a discrete grid, it just solves thecontinuous light propagation equation and, therefore, the solutionis also a continuous function.
2.2. Navigation in outdoor environments
The aim of the research presented in this paper focuses onoutdoor environments. This section summarizes motion planningalgorithms that can be applied outdoors.
In 1991,Mitchell and Papadimitriou [11] proposed an algorithmfor determining the shortest path between a source and adestination through a planar subdivision in which each region hadan associated weight. Distances were measured according to aweighted Euclidean metric: each region of the subdivision had aweight associated with it, and the weighted distance between twopoints in a convex region was the product of the correspondingweight and the Euclidean distance between them. In that paper,the total length of a path is defined to be the weighted sumof (Euclidean) lengths of the subpaths within each region, andthe continuous Dijkstra algorithm is used for path planning. Thepaper gives interesting proofs, but it is mainly theoretical and noexperimental results are presented.
Instead of considering a continuous environment, some workspropose the use of a discretization algorithm before the pathplanning. Guo et al. [12], for example, use a regular grid andthe A∗ algorithm in simulations. Another approach, see [13], usesa framed quadtree and the D∗ algorithm for path planning. Inaddition to decomposing the environment, the path planningneeds a cost metric for each region.
For indoor navigation, other vector field approaches that areable to compute continuous fields on a discrete environment havebeen proposed (Conner et al. [14] in 2003 and Lindemann andLaValle [15] in 2005). All of them could, at first, be adapted toallow outdoor navigation as proposed in this paper. However,the main motivation for seeking a new methodology, that in theend became simpler and computationally more efficient than theprevious ones, was the necessity to directly and automaticallyincorporate some properties of the terrain into the computation,thus generating bounded speed profiles for the robot.
In our paper, we are particularly interested in using the terrainroughness, the gradient, and the height as a metric. Similarly to
108 S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114
START START
FINISH FINISH
Fig. 2. Dijkstra’s method gives multiple short paths (left image), whilst the Fast Marching method gives the optimal diagonal path (right image).
other outdoor navigation works, our approach decomposes theenvironment into a triangular grid well adapted to the character-istics of the terrain. The environment is decomposed using theConstrained Delaunay Triangulation (CDT) and then the numberof faces is reduced while attempting to preserve the overall shapeof the original surface. An important advantage of CDT over regu-lar grid and quadtree representations is that it usually generates amuch smaller number of cells when representing complex struc-tures typical of outdoor environments.
Among all the previous works, our work is closely related toGuo et al. [12] in 2003, Yahja et al. [13] in 2000, and Mitchell [11]in 1991, but with important differences. First, similarly to Guoet al. [12], our method computes cost values for each cell of adecomposed map, but our method uses a continuous version ofDijkstra’smethod and solves the partial differential equation of thelight (Eikonal equation). Second, we work with continuous mapslike in Mitchell [11], and our path is obtained on them. Only theinitial data are given on discretemaps. However, instead of regularor quadtree decomposition, we decompose the environment usingCDT [16]. Similar to quadtree, CDT is a non-uniform decompositionthat yields in high resolution cells in the complex regions of theenvironment. An important advantage of CDT over the quadtreerepresentation is the smaller number of cells when representingnon-polygonal, highly complex structures, common in outdoorsenvironments.
3. The Eikonal equation and the Fast Marching planningmethod
In robotics, the path planner of the mobile robot must driveit in a smooth and safe way to the goal point. In nature, thereare phenomena with the same way of working: electromagneticwaves. If at the goal point there is an antenna that emits anelectromagnetic wave, then the robot could drive himself to thedestination following the waves to the source. The concept ofthe electromagnetic wave is especially interesting because thepotential has all the good properties desired for the trajectory, suchas smoothness (that is, C∞) and the absence of local minima.
The moving boundary of a disturbance is called a wave front,and it can be described by the Eikonal equation [17]. The Eikonal(from theGreek ‘eikon’, whichmeans ‘image’) is the phase functionin a situation forwhich the phase and amplitude are slowly varyingfunctions of the position. Constant values of the Eikonal representsurfaces of constant phase, or wave fronts. The normals to thesesurfaces are rays (the paths of energy flux); thus, the Eikonalequation provides amethod for ‘‘ray tracing’’ in amediumof slowlyvarying index of refraction (or the equivalent for other kinds ofwaves).
One way to characterize the position of a front in expansion isto compute the time of arrival T , in which the front reaches eachpoint of the underlying mathematical space of the interface. It isevident that, for one dimension, we can obtain the equation of the
arrival function T in an easy way, simply considering the fact thatthe distance θ is the product of the speed F and the time T :
θ = F · T . (1)
The spatial derivative of the solution function becomes thegradient
1 = FdTdθ
(2)
and therefore, themagnitude of the gradient of the arrival functionT (θ) is inversely proportional to the speed:
1F
= |∇T | . (3)
For multiple dimensions, the same concept is valid because thegradient is orthogonal to the level sets of the arrival function T (θ).In this way, we can characterize the movement of the front as thesolution of a boundary conditions problem. If speed F depends onlyon the position, then Eq. (3) can be reformulated as the Eikonalequation:
|∇T | F = 1. (4)
The FMmethod is a numerical algorithm for solving the Eikonalequation, originally on a rectangular orthogonal mesh introducedby Sethian in 1996 [18]. The FM method is an O(n) algorithm, asdemonstrated in [19], where n is the total number of grid points.The scheme relies on an upwind finite difference approximation tothe gradient and a resulting causality relationship that lends itselfto a Dijkstra-like programming approach.
The FMmethods are designed for problems in which the speedfunction never changes sign, and so the front is always movingforward or backward (there are no reflections, interferences, ordiffractions). This allows us to transform the problem into astationary formulation, because the front crosses each grid pointonly once. This conversion to a stationary formulation, in additionto a whole set of numerical tricks, gives it its tremendous speed.
Since its introduction, the FM approach has been successfullyapplied to a wide variety of problems that arise in geometry, me-chanics, computer vision, and manufacturing processes (see [20]for details). Numerous advances have been made to the originaltechnique, including the adaptive narrow band methodology [21]and the FM method for solving the static Eikonal equation [18].See [20] for further details and summaries of level set and FM tech-niques for numerical purposes.
3.1. Algorithm implementation on an orthogonal mesh
The FMmethod applies to phenomena that can be described asa wave front propagating normal to itself with a speed functionF = F(i, j). The main idea is to methodically construct the solutionusing only upwind values (the so called entropy condition).
S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114 109
a
d
b
e
c
f
Fig. 3. Scheme of Fast Marching propagation.
Let T (i, j) be the solution surface T (i, j) at which the curve crossesthe point (i, j); then, it satisfies |∇T |F = 1, the Eikonal equation.
This equation is applied to grid points, which are classified intothree different types: alive, trial, and far, see Fig. 3.
• Alive Points (blue points) are points where values of T areknown.
• Trial Points (gray points) are points around the curve (alivepoints) where the propagation must be computed. The set oftrial points is called narrow band. To compute the propagation,points in the narrow band are updated to alive points, while thenarrow band advances.
• Far Away Points (white points) are points where the propaga-tion was not computed yet. During the propagation, far awaypoints are converted to trial points.
It is important to observe that the propagation occurs fromsmaller to greater values of T . Fig. 3 explain this idea: the bluepoint (alive) represents the initial curve; in Fig. 3(b), the valueof T is computed in the neighborhood of the blue point; thisneighborhood is converted from far away (white) to trial points(gray); in Fig. 3(c), the trial point with the smallest value of T ischosen (for example, ‘‘A’’); in Fig. 3(d), the values of T are computedin the neighbors of point A, converting them from far away totrial points. In Fig. 3(e), the trial point with the smallest value ofT is chosen (for example, ‘‘D’’); in Fig. 3(f), the neighbors of D areconverted from far away to trial points. And so on.
3.2. Algorithm implementation on a triangulated mesh
As previously stated, since the work presented in this paperfocuses on outdoor environments, the FM method is going tobe applied on a triangulated mesh instead of on an orthogonalone. Sethian in several works [20,22] extended the method totriangulated domains with the same computational complexity.
4. MatrixW : the viscosity map
As stated in Section 1, the direct application of the FM methodstill has some problems. The most important one that typicallyarises in mobile robotics is that optimal motion plans may bringrobots too close to obstacles, which is not safe.
In our approach, this problem has been solved in the sameway as nature does: the electromagnetic waves, as light, have apropagation speed that depends on themedia, that is the slownessindex of the front wave propagation of a medium. In our case, therefraction index is defined by the viscosity map.
The proposed technique is based on the FM method, changingthe speed of the wave front using a potential surface generatedfrom the 3D environment characteristics and the robot limitations.By doing so, the method changes the time when the front reacheseach point and when the generated trajectory is calculated. Thistrajectory is not going to be the simple geodesic, but it is going tobe modified according to the robot and task needs. To be able tomodify this speed, the proposed method creates a weight matrixW , which is currently built based on three main characteristics ofthe 3D surface: the spherical variance, the saturated gradient, andthe height. Some other characteristics can be added to the methodand it will build a different potential surface.
4.1. Spherical variance
The spherical variance [23] consists of finding the roughnessof a surface to determine whether it is crossable or not. In [24]a method to calculate the roughness degree is presented. Thismethod is based on the normal vector deviation in each point ofthe surface. The spherical variance is obtained from the orientationvariation of the normal vector in each point. The study uses thefollowing reasoning:• In a uniform terrain (low roughness), the normal vectors in a
surface will be approximately parallel and, for this reason, theywill present a low dispersion (see Fig. 4, left).
• On the other hand, in an uneven terrain (high roughness) thenormal vectors will present great dispersion due to changes intheir orientation (see Fig. 4, right).
The spherical variance is obtained as follows:1. Given a set of n normal vectors to a surface, defined by their
three components−→Ni = (xi, yi, zi), the module of the sum
vector R is calculated by:
R =
ni=0
xi
2
+
n
i=0
yi
2
+
n
i=0
zi
2
. (5)
110 S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114
Fig. 4. Spherical variance analysis. Left: a uniform surface shows parallel normalvectors. Right: an uneven surface shows normal vectors with different orientations.
2. Next, the mean value is normalized by dividing the module Rbetween the number of data n, so the value of the result iswithin [0, 1]:
Rn
∈ [0, 1] . (6)
3. Finally, the spherical variance ω is defined as the complemen-tary to the normalized mean vectors module:
ω = 1 −Rn. (7)
When ω = 1, there exists a maximum dispersion that can beconsidered as the maximum roughness degree, and when ω = 0,a full alignment exists and the terrain will be completely flat.
4.2. Saturated gradient
In vector calculus, the gradient of a scalar field is a vector fieldwhich points in the direction of the greatest rate of increase of thescalar field, and whose magnitude is the greatest rate of change.
Consider a surface whose height above sea level at a point (x, y)is H(x, y). The gradient of H at a point is a vector pointing in thedirection of the steepest slope or grade at that point. The steepnessof the slope at that point is given by the magnitude of the gradientvector.
The gradient of a scalar function f (x1, x2, . . . , xn) is denotedby ∇f or
−→∇ f , where ∇ (the nabla symbol) denotes the vector
differential operator. The gradient of f is defined to be the vectorfield whose components are the partial derivatives of f . That is:
∇f =
∂ f∂x1
, . . . ,∂ f∂xn
. (8)
The saturated gradient consists of giving a limit value to thegradient of each point over the 3D surface. It means that, if thegradient value exceeds that limit, the point will not be included inthe list of accessible points determined by the robot limitations.The gradient depends on the robot capabilities; the maximuminclination that the robot is able to cross will be the limit value forthe saturated gradient.
4.3. Construction of matrix W
As previously explained, with this matrix the algorithm canmodify the path that the robot is going to follow across the 3Dsurface. The way the matrix modifies the path is by giving aviscosity value for each point on the surface. It means that thepropagation speed of the front end of the FM wave is modified.Hence, the time when the wave reaches each point will depend onthat viscosity. We can add as many characteristics as we need toget different paths. These characteristics will modify the viscosityat each point.
The saturated gradient, the spherical variance, and the heightare three matrices G, Sv, and H with the same length as the vertex
matrix (the 3D mesh). The value of each vertex of the 3D grid willbe determined by the calculated gradient, spherical variance, andthe height of each point.
To build matrix W we give a weight or ponder factor to eachsurface characteristic and we can determine which one is themostimportant depending on the task requirements.
The values of the three matrices vary from 0 to 255, so thevalues of matrix W are also within this range. The componentsof matrix W with a value of 0 will be points in the vertex matrixwith maximum speed. Hence, these are points which the robotcan cross without any problem and at its maximum speed. Thecomponents of W with a value of 255 will be points in the vertexwith a minimum speed. This means that the robot will not be ableto pass across them:
W = a1 · G + a2 · Sv + a3 · H (9)
where:i
ai = 1. (10)
The values of these ponder factors ai are selected considering thefeatures of the surface that want to be penalized. Those featurescould be penalized depending on the task requirements or the robotlimitations. For example, a high value of a1 implies that the pointswhere the inclination of the surface (the gradient) is high will beavoided by the path.
After matrixW is generated, themethod runs the FM algorithmover the modified mesh (3D mesh +matrix W ) to calculate thebest trajectory. With the FM method the path found will be theleast time path. In the normal FM evolution, this path will be theshortest because all the points in the surface will have the same‘‘speed’’ for the front propagation. With matrix W , the proposedmethod changes that ‘‘speed’’, since this matrix gives informationabout the difficulty to pass through each point of the surface. Thetrajectory will be modified depending on the surface conditionsand characteristics and according to the robot limitations. Since themethod modifies the ‘‘speed’’ of the Fast Marching wave, it givesnot only the best trajectory, but also the speed to control the robot.
5. Algorithm simulations
5.1. Path planning on surface images
As previously stated, in the proposed method an image of theenvironment is needed. In relation to the outdoor environmentreconstruction, there are many ways to build an environment andrepresent it as a 2 1
2 or 3D surface. This surface can be build fromsensor data, elevation maps, bitmap images, etc.
The proposed method bases the surface reconstruction onbitmap images. The image contains the coordinate of each pixeland a number which indicates the value on a scale from 0 to 255.This value gives us the height of each point from the map. Sincethe method is working in 3D, there are 3 matrices, one for eachcoordinate X , Y , and Z . X and Y are the coordinates for a plainsurface and Z is the height of each point. In order to create atriangular mesh, the algorithm reads the data from the bitmap fileto create these three matrices X , Y , and Z and then it builds a plainmesh based on X and Y coordinates. The algorithm generates aDelaunay triangulation [25] that is equivalent to the nerve of thecells in a Voronoi Diagram [26]. Once the triangulation is built,the next step of the algorithm is to modify the plain surface byadding the third coordinate.With this step the height of each pointis changed for the real value acquired from the bitmap.
After the mesh is created, the algorithm is able to extract theneeded data, the vertices and the faces of the triangles, from thematrices.
S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114 111
Fig. 5. 3D surface built from a bitmap file.
Fig. 6. 3D surface with triangular mesh.
From this point, the algorithm proposed works as follows:
1. The G, Sv, and H matrices are calculated as described in theprevious section.
2. The difficulty matrix W is obtained.3. A goal point is selected.4. The Fast Marching potential field is calculated as a wave expansion
from the initial point.5. The path is obtained by following the direction defined by the
descent gradient on the Fast Marching potential field, until the goalpoint is reached.
For this experiment, the image shown in Fig. 5 is used. This surfacecorresponds to an area in the White Mountains of New Hampshire,USA. This image is a Digital Elevation Model (DEM) for the Mt.Washington quadrangle obtained from the United States GeologicalSurvey (USGS) [27]. Following the process previously described, thealgorithm is able to model the 3D surface, as can be observed in Fig. 6.
Next, several paths over the surface already presented will beobtained between the same initial and final points.
In these images, the initial and the final points have beensituated over the top of the two sides of themountain range. Thosepaths are obtained by varying the values of the weight factors ai ofmatrix W .
Let us first show the path obtainedwithout using theW matrix.This path, as shown in Fig. 7, is the shortest path between theinitial and final points since every point in the mesh has the sameviscosity or difficulty value.
The fact that every point has the same viscosity value can beexplained by observing the wave front propagation in this case. InFig. 8, the wave front propagation of the FMmethod without usingmatrix W is shown. The width of each color band determines thespeed of the wave front propagation. At the point where the speedis high, thewave front expands faster and, therefore, the color bandiswider. On the other hand, if the speed is limited, thewave front isslower and, as a consequence, the color band is thinner. In Fig. 8, it
Fig. 7. Geodesic calculated without using matrixW .
Fig. 8. Wave front propagation without using matrixW .
Fig. 9. Path calculated whenW = A.
can be observed that every color band has the same size, therefore,the wave speed is equal for every point of the surface.
In the case that W = A, this implies that the difficulty of thepath will be determined by the height of every point of the mesh.In Fig. 9, the path obtained when the height is penalized, withoutconsidering the roughness of the surface or its inclination, is pre-sented. As can be observed, the calculated pathwill try to reach thefinal point passing through the deepest part of the valley.
On the other hand, if we decide to calculate the path penalizingjust the inclination of the surface, then the viscosity matrix isdefined as W = G. In this case, as shown in Fig. 10, the path willfollow the top of the mountain chain, since, although the terrainseems very uneven, its slope is lower than the hillside one.
Another path is obtained when W = Sv, that is, when justthe roughness of the surface is considered. In this case, the pathobtained will try to follow a ‘‘soft’’ trajectory. In fact, as shown inFig. 11, the path is similar to the one obtained when the height ispenalized. This is because, as observed in the figures, the roughnesson the top of themountain chain ismuch bigger than the one of the
112 S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114
Fig. 10. Path calculated when W = G.
Fig. 11. Path calculated when W = Sv.
Fig. 12. Path calculated usingW = 0.15 ∗ A + 0.05 ∗ Sv + 0.85 ∗ G.
slopes of the hillsides of the valley. Therefore, the path calculatedis the one expected.
Finally, the general idea proposed in this paper is the possibilityof combining the three matrices in order to obtain a path thatconsiders the height A, the roughness Sv, and the inclination G ofthe surface. In the previous figures, it can be observed that, forthe selected initial and final points, the height and the sphericalvariance matrices favor that the path goes down the valley. On theother hand, the gradient matrix favors the path through the topof the mountain chain. Therefore, we can select the values of eachweight factor ai in order to consider the limitations or features ofthe robot used.
Figs. 12 and 13 show two views of the same path obtainedwhenW = 0.15 ∗ A + 0.05 ∗ Sv + 0.85 ∗ G.
Moreover, the values of the weight factors ai can be changed ifthe robot to be used is different or modified. It is also importantto note that the trajectories calculated are a tentative pathfor the robot. The path can be modified on line by modelingthe environment with the robot sensors and recalculating thetrajectory in a local area.
Fig. 13. Path calculated usingW = 0.15 ∗ A + 0.05 ∗ Sv + 0.85 ∗ G.
Table 1Computational cost (seconds).
Number of vertices 3125 5190 9300
Fast Marching time 0.05 0.09 0.18Path extraction 0.25 0.40 0.74
Total time 0.30 0.49 0.92
Fig. 14. Path calculated on a 3D object usingW = 0.9 ∗ A + 0.1 ∗ Sv.
In relation to the time consumption of the process, in Table 1the computational costs in seconds are shown. The values showncorrespond to the obtained paths between the same initial and finalpoints (over the same surface), and for different meshes (differentnumber of vertices). As observed, the total cost of the process, whichincludes the time taken by the ‘‘wave expansion’’ phase and the pathextraction, is very low (less than 1 s), evenwhen the number of verticesof the mesh is high. Therefore, we can consider that the algorithmproposed in this paper can be used as an on-line method.
5.1.1. Test on 3D imagesAs shown in the previous section, the main application of the
algorithm is the calculation of paths for robot navigation in outdoorenvironments. Another application could be its use for calculatingthe path for climbing robots in more complex environments. Inthis case, since the robot could be required to climb verticalwalls, the saturated gradient is not considered in the difficultymatrixW . In order to prove the usefulness of the algorithm in thiskind of surface, the following simulations have been made. Theproposed algorithm has been used to calculate the path betweentwo selected points situated on the 3D objects shown in Figs. 14and 15.
In Fig. 14, the path has been calculated considering W = 0.9 ∗
A + 0.1 ∗ Sv. This implies that the difficulty matrix W gives more
S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114 113
Fig. 15. Path calculated on a 3D object usingW = 0.8 ∗ A + 0.2 ∗ Sv.
Fig. 16. Starting point. Both planned paths cross.
importance to matrix A corresponding to the height of every point.Therefore, as can be observed, the calculated path goes under thestructure in order to reach the final point.
Another object is shown in Fig. 15, where the calculated pathalso tries to go through the lower part of the object since W =
0.8 ∗ A + 0.2 ∗ Sv. In both figures, it is shown how the proposedalgorithm is also useful in 3D objects.
5.2. Dynamic evolution of the paths of two robots approaching
Another interesting application of the proposed method is theobtaining of the dynamic trajectories of two robots which navigateapproaching each other. In this case, another situation within thecontext of project Robauco is considered. The idea is that a UAV takesan image of the surface and sends it to both robots. Then, each robotcalculates the right path to get to their goals using the proposedalgorithm. In order to get these trajectories, each robot maintainsa map with the matrix of difficulty of the terrain. In this map, it isnecessary to add a small Gaussian to the position of each robot inorder to avoid possible collisions. The position of each robot can beobtained on their own using GPS signals and, when they are separatedby a reasonable distance, they communicate their respective positionsby radio.
This bivariable Gaussian distribution is centered at the observedposition of each robot with a standard deviation of 10 in thedirection of the trajectory (longer vector), and of 3 in the seconddirection (shorter vector, orthogonal to the longer vector). ThisGaussian has been rescaled with a factor of 10 so that it can havethe necessary importance in the difficulty map.
In the initialization phase, the algorithm calculates the generaldifficulty matrix or viscosity mapW .
Then, at each iteration, a GaussianGi(k) is added to the observedposition of each robot i. Afterwards, at each iteration, the trajectoryof the robot i is calculated using the described Fast Method on themapW + Gi(k), until the goal point is reached.
Fig. 17. Robots approaching with no view of each other.
Fig. 18. Robots approaching sensing their mutual position.
Fig. 19. Robots moving away from each other.
In the following figures the dynamic evolution of the trajecto-ries of two robots is shown. The goal of each robot is representedby two red dots (one of them situated in the upper-right cornerand the other one in the bottom-left corner), while the current po-sition is represented by two blue ones. The starting point of eachrobot, as shown in Fig. 16, is almost the same goal point of the otherone. Therefore, the first planned paths cross each other at severalpoints, provoking potential collisions. As the robots move towardstheir goals, they approach each other. In Fig. 17 we observe the sit-uation where both robots are approaching but they are not awareof the position of the other. In this case, a collision of both robots isexpected. Later, when the robots are closer, they start sensing eachother and, as can be seen in Fig. 18, their paths run in parallel withno collision. Finally, in Fig. 19 it is observed that each robot movesaway from the other till they reach their goal points. As has justbeen explained, this robot avoidance is made thanks to themodifi-cation of the difficulty matrixW by adding a Gaussian to the givenposition of the robot.
114 S. Garrido et al. / Robotics and Autonomous Systems 61 (2013) 106–114
6. Conclusions
The algorithmwe have presented here is a newway to calculatetrajectories formoving a robot over a 3Doutdoor surface. Onemainpoint about the proposed method is that it can be used not onlyas a Path Planning method, but also to control the robot speedto keep it within a range given by the limit speed allowed overthe 3D surface, taking into account environmental characteristicsand task requirements (shortest path, lower consumption, etc.)reflected in matrix W . There are two values that can be attainedfrom the results of the algorithm: the robot speed and the robotorientation. The speed is taken from the potential surface and theorientation can be taken from the next point in the trajectory thatis going to be occupied by the robot. If the robot orientation andthe next point where the robot is going to be are known, we cancalculate the control law that has to be given to the robot in orderto make it reach that next point. The most important thing aboutthis algorithm is that it works in real time. It is really fast and givesus the possibility to use it on-line to make decisions in order toavoid fixed or moving obstacles.
References
[1] Robauco, Robauco project, 2007. http://www.robauco.es/.[2] S. Garrido, L. Moreno, D. Blanco, Exploration of a cluttered environment using
voronoi transform and Fast Marching method, Robotics and AutonomousSystems 56 (12) (2008) 1069–1081.
[3] S. Garrido, L.Moreno,M. Abderrahim, D. Blanco, Fm2: a real-time sensor-basedfeedback controller for mobile robots, International Journal of Robotics andAutomation 24 (1) (2009) 3169–3192.
[4] E. Dijkstra, A note on two problems in conexion with graphs, NumerischeMathematik 1 (1959) 269–271.
[5] N. Nilsson, Principles of Artificial Intelligence, Tioga Publishing Company, PaloAlto, CA, 1980.
[6] A. Stentz, The focused D∗ algorithm for real-time replanning, in: Proceedingsof the International Joint Conference on Artificial Intelligence, IJCAI, 1995.
[7] M. Likhachev, G. Gordon, S. Thrun, Advances in Neural Information ProcessingSystems, MIT Press, 2003 (Chapter ARA*: anytime A∗ with provable bounds onsub-optimality).
[8] S. Koenig, M. Likhachev, Improved fast replanning for robot navigation inunknown terrain, in: Proceedings of the IEEE International Conference onRobotics and Automation, ICRA, 2002.
[9] A. Nash, K. Danie, S. Koenig, A. Felner, Theta*: any-angle path planning on grids,in: Proceedings of the AAAI Conference on Artificial Intelligence, AAAI, 2007,pp. 1177–1183.
[10] D. Ferguson, A. Stentz, Advances in Telerobotics, Springer Berlin, 2007.[11] J.S.B. Mitchell, C.H. Papadimitriou, The weighted region problem: finding
shortest paths through a weighted planar subdivision, Journal of theAssociation for Computing Machinery 38 (1) (1991) 18–73.
[12] Y. Guo, L.E. Parker, D. Jung, Z. Dong, Performance-based rough terrainnavigation for nonholonomic mobile robots, in: Proceedings of the IEEEIndustrial Electronics Society, 2003, pp. 2811–2816.
[13] A. Yahja, S. Singh, A. Stentz, An efficient online path planner for outdoormobilerobots, Robotics and Autonomous Systems 32 (2000) 129–143.
[14] D. Conner, A. Rizzi, H. Choset, Composition of local potential functionsfor global robot control and navigation, in: Proceedings of 2003 IEEE/RSJInternational Conference on Intelligent Robots and Systems, IROS 2003, 2003,pp. 3546–3551.
[15] S. Lindemann, S. LaValle, Smoothly blending vector fields for global robotnavigation, in: Proceedings of the 44th IEEE Conference on Decision andControl, 2005, pp. 3553–3559.
[16] J.R. Shewchuk, Applied computational geometry: towards geometric engi-neering, 1996 (Chapter Triangle: engineering a 2D qualitymesh generator andDelaunay triangulator).
[17] J.L. Davis, Wave Propagation in Solids and Fluids, Springer, 1988.[18] J.A. Sethian, Theory, algorithms, and applications of level set methods for
[19] L. Yatziv, A. Bartesaghi, G. Sapiro, A fast O(n) implementation of the FastMarching algorithm, Journal of Computational Physics 212 (2005) 393–399.
[20] J. Sethian, Level Set Methods, Cambridge University Press, 1996.[21] D. Adalsteinsson, J. Sethian, A fast level set method for propagating interfaces,
Journal of Computational Physics 118 (2) (1995) 269–277.[22] R. Kimmel, J.A. Sethian, Computing geodesic paths on manifolds, The National
Academy of Sciences 95 (1998).[23] K. Mardia, P. Jupp, Directional Statistics, Wiley Series in Probability and
Statistics, 1999.[24] Traversable region modeling for outdoor navigation, Journal of Intelligent and
Robotic Systems, 43 (2–4).[25] D.T. Lee, B.J. Schachter, Two algorithms for constructing a Delaunay
triangulation, International Journal of Computer and Information Sciences 9(1980) 219–242.
[26] A. Okabe, B. Boots, K. Sugihara, Spatial Tessellations: Concepts and Applica-tions of Voronoi Diagrams, Chichester, UK, 1992.
[27] United states geological survey, 2012. http://eros.usgs.gov [online], [cited2012-09-25].
Santiago Garrido is an Associate Professor at Carlos IIIUniversity ofMadrid. He received theB.Sc. inmathematics,and the B.Sc. in physics in 1995. He received hisPh.D. degree in robotics and control in 2000 fromthe Carlos III University. His research interests arein the areas of mobile robotics, mobile manipulators,environment modeling, path planning and mobile robotglobal localization problems.
María Malfaz received the B.Sc. degree in physics from LaLaguna University, La Laguna, Spain, in 1999, and theM.Sc.degree in control systems from the Imperial College ofLondon, London, UK, in 2001. She received the Ph.D. degreein industrial engineering from the Carlos III University ofMadrid, Madrid, Spain, in 2007, with the topic ‘‘DecisionMaking System for Autonomous Social Agents Based onEmotions and Self-learning’’. She is currently an AssistantProfessor in the Department of System Engineering andAutomation of the Carlos III University of Madrid, Madrid,Spain. Her current research area follows the line carried
out in her thesis.
Dolores Blanco received the B.S. degree in physics fromthe University Complutense of Madrid, Spain in 1992. Shereceived the Ph.D. degree in mechatronics from CarlosIII University of Madrid (UC3M) in 2002. From 1996 to1999, she was a fellowship student at the Departmentof Systems Engineering and Automation of UC3M. Since2009, she is associate professor in the same department.She is a member of the Robotics Lab Group at UC3M.Her current research includes sensor based path planning,localization and control for mobile manipulators andemerging actuator technologies for rehabilitation robotics.
