Outdoor Motion Planning Using Fast MarchingLukas De Sanctis, Santiago Garrido, Luis Moreno and Dolores Blanco

Robotics Lab.Carlos III University of Madrid, Spain

Email: lsanctis, sgarrido, moreno, dblanco@ing.uc3m.es

Abstract— In this paper is presented a new path planningmethod for tridimensional environments based on Fast Marchingtechniques. The proposed method applies Fast Marching toa 3D surface created with a triangular mesh to calculate asmooth trajectory from one point to another. The method usesa triangular mesh instead of a square mesh to improve thesmoothness of the trajectory. Before running the algorithm themethod calculates a weight matrixW based on a potential surfacegenerated from the 3D surface characteristics such as gradient,spherical variance and the Robot limitations, these matrix is usedto limit the speed propagation of the Fast Marching wave in orderto find the best path depending on the task requirements, e.g. theless energy consumption path, the fastest path, the most plainterrain. The algorithm also gives us a control signal for the robotbased on the wave front propagation speed and this speed canbe used to control the robot.

I. INTRODUCTION

Path Planning for mobile robots has many ways to becalculated such as Potential field methods that consist inattractive an repulsive forces that guides the robot to the goalwhile pushing it away from the obstacles, they consider therobot as a point, Exact cell decomposition methods generatesregions covering free space and creates cell connectivity in agraph, Approximate cell decomposition that is similar to theexact method but the generated cells are predefined shapesand don’t cover the free space exactly, Roadmap methodsrepresents the environment with a node based network and findthe path with graph search algorithms [?]. All these methodsare used to solve the Path Planning problem over 2D surfaces,they can not be applied to 3D surfaces because there are manycharacteristics that need to be taken into account like gradient,viscosity, spherical variance, etc. That is why we present thisnew method, because we need to solve the problem in 3Dsurfaces and control a robot in outdoor environments.

If we want to calculate a trajectory from one point to anotherthe first thing needed is a global map of the environment,starting from these point we are able to calculate a tentativetrajectory.

To be able to calculate better trajectories over a global map,in these paper is presented a new Path Planning method thatapplies Fast Marching methods on a potential surface createdfrom the 3D surface characteristics with a gradient saturationmethod, the gradient saturation is based on a weight matrixW that may be changed depending on the robot capabilitiesand the environment complexity.

Fast Marching technics has been used by Path Planningmethods to calculate trajectories from one point to another,

but the generated trajectories aren’t safe and smooth, thesetrajectories are the shortest path and they are to close tothe obstacles [Sethian:1]. These methods are used in 2Denvironments, the method presented adds another dimension tothe problem and manipulate the 3D Fast Marching algorithmin order to modify the trajectories obtained to get a better andsafer trajectory.

The new method algorithm is applied over a surface that isconstructed with a triangular mesh, these give us the possibilityto calculate smoother trajectories than those calculated withsquare meshes, and it also allow us to generate realisticsurfaces due to the capabilities of the triangles to fit betterto the characteristics of the map.

After having the global map the presented method extractsthe data to be able to calculate the gradient and the sphericalvariance, then it combines these data with the robot limitationsto generate the potential surface and from that potential get theweight matrix, once the matrix is ready, the method appliesthe Fast Marching algorithm over a 3D surface to generatethe path, if the W matrix is not used the trajectory that isobtained will be just the geodesic. Applying the W matrix theproposed method also give us a control signal based on theFast Marching wave speed propagation in order to be able tocontrol the robot over the 3D surface using a motion planner[?], [?] also based on Fast Marching technics.

This method can also be used on 2D environments anddoesn’t have to be only used to find global trajectories, theglobal trajectory can be modified on-line by combining theinitial trajectory with new local trajectories calculated basedon the sensor data of the robot, which allows the robot to beable to avoid fixed obstacles that haven’t been added to theglobal map or moving objects that appear on the march.

II. THE FAST MARCHING METHOD

The Fast Marching Method is a numerical algorithm forsolving the Eikonal equation on a mesh. Sethian introduces theFast Marching Method in 1996 [?]. It applies to phenomenathat can be described as a wave front propagating normal toitself with a speed function F = F (i, j). The main idea is tomethodically construct the solution using only upwind values(the so called entropy condition). Let T (i, j) be the solutionsurface T (i, j) at which the curve crosses the point (i, j), thenit satisfies the equation 1, the Eikonal equation.

|∇T |F = 1 (1)

Fig. 1. Bitmap file to build the 3D map

The physical meaning is that the gradient of arrival time ofthe front is inversely proportional to the speed of the front. Theequation is applied to grid points, these points are classifiedin three different types: alive, trial and far.• Alive points are points where values of T are known.• Trial points are points around the alive points, where the

propagation most be computed, These set of points isknown as narrow band, after compute propagation, pointsin the narrow band became alive points while the narrowband advances.

• Far points are the points away from the narrow band,while the wave front advances the closest Far pointsbecame trial points.

The propagation occurs from smaller to greater values ofT , the curve propagation begins in one point (alive point) thevalue of T is computed in the neighbor points and these pointsare converted from far points to trial points, the trial point withsmaller value is chosen to be converted to alive point and theT values are computed in the neighbors of these new pointconverting these set of points in trial points and so on.

III. ENVIRONMENT RECONSTRUCTION

There are many ways to build an environment and representit as a 3D surface, these surface can be build from sensor data,elevation maps, bitmap images, etc. The proposed methodbases the surface reconstruction on bitmap images, what wehave in the image is the coordinate of each pixel and a numberthat indicates the gray value on a scale from 0 to 255, thesevalue gives us the height or the z coordinate of the map.

The algorithm reads the data from the bitmap file(Figure1) and creates tree matrices X , Y and Z, now it has the 3Dcoordinates for all the surface, to be able to reconstruct thatsurface it is necessary to process these matrices to build thetriangular mesh and from that mesh the algorithm gets thevertex and faces of each triangle.

A. 3D Triangular Mesh

As the method is working in 3D there are 3 matrices, onefor each coordinate X , Y and Z, X and Y are the coordinatesfor a plain surface and Z is the height of each point, in orderto create a triangular mesh the algorithm has to create a plainmesh based on X and Y coordinates, the algorithm generates a

Fig. 2. 2D Triangular Mesh

Fig. 3. 3D Surface with Triangular Mesh

Delaunay triangulation [?] that is equivalent to the nerve of thecells in a Voronoi Diagram [?]. In Figure 2 is the 2D triangularmesh, now that the triangulation is builded the next step ofthe algorithm is modify the plain surface by adding the thirdcoordinate, with these step the height of each point is changedfor the real value acquired from the bitmap. After the meshis created the algorithm is able to extract from the matricesthe needed data, the vertex and the faces of the triangles, andwhit these values the algorithm is able to model the 3D surface(Figure 3) where the proposed method is going to acquire the3D surface characteristics and build the potential surface where

Fig. 4. 3D Surface builded from a bitmap file

the Fast Marching method is going to be applied. The 3Denvironment has being build from a lower resolution bitmapin order to show the triangular mesh, the real surface withoutshowing the triangular mesh is shown on Figure 4.

IV. ALGORITHM IMPLEMENTATION

The proposed method is based on the Fast Marching Methodthinking of changing the speed of the wave front using apotential surface generated from the 3D environment charac-teristics and the robot limitations, by doing these the methodis changing the time in which the front reaches each point andwhen the generated trajectory is calculated, it is not going tobe the simple geodesic, the trajectory is going to be modifiedaccording to the robot and the task needs. To be able to modifythis speed the proposed method creates a weight matrix W ,the matrix is builded based on two main characteristics of the3D surface, the saturated gradient and the spherical variance,some other characteristics can be added to the method and itwill build a different potential surface.

A. W Matrix Construction

With this matrix the algorithm can modify the path that therobot is going to follow across the 3D surface, the way the ma-trix modifies the path is because it gives a viscosity value foreach point on the surface, it means that the speed propagationof the front end of the Fast Marching wave is modified hencethe time in which the wave reaches each point will dependon that viscosity, we can add as many characteristics as weneed to get different paths, these characteristics will modifythe viscosity at each point.

1) Spherical variance: The spherical variance [?] consistsin founding the roughness of a surface to determine if itscrossable or not, to do these we have tu calculate the normalvector to each triangle on the surface

−→N = (x, y, z) and we

have an {−→Ni} set of neighbor vectors, the spherical variance

is defined as an statistical ω 2, defined as the complement ofthe normalized module of the vector set.

ω = 1−R

n(2)

Given a set of n normal vectors, defined by its tree com-ponents

−→Ni = (xi, yi, zi), the module of the vector set is

calculated as shown in equation 3.

R =

√√√√(n∑

i=0

xi

)2

+

(n∑

i=0

yi

)2

+

(n∑

i=0

zi

)2

(3)

These value is divided by the number of vectors n tonormalize the values into a range between 0 and 1 4.

R

n∈ [0, 1] (4)

The spherical variance is interpreted as the dispersal of thevectors, if ω = 0 there is a maximal variance, that means thatthe surface has a maximum of roughness, and when ω = 1there is a complete alignment of the vectors and the surfaceis completely flat.

2) Saturated Gradient: The saturated gradient consists ingiving a limit value to the function when calculating thegradient of each point over the 3D surface, it means that ifthe founded value excedes that limit will not be accepted asa valid point and will not be included in the list of accesiblepoints determined by the gradient. The gradient will dependon the robot capabilities, the maximum inclination that therobot is able to cross will be the limit value for the saturatedgradient.

The saturated gradient and the spherical variance are goingto be two matrices G and Sv with the same length as thevertex matrix, that is because each vertex value will have itsgradient and spherical variance assigned.

To build the W matrix we give a weight factor in thealgorithm to each surface characteristic and we can determinewhich one is most important depending on the task require-ments, in these case the proposed method uses G and Sv butremember that we can add as many characteristics as we need.The matrixW will have values from 0 to 255, the componentsin the W matrix with a 0 value will be points in the vertexmatrix with maximum speed, hence these are points whichthe robot can cross without any problems and at its maximumspeed, the components inW with a value of 255 will be pointsin vertex with a minimum speed, this means that the robot willnot be able to pass across them.

W = aG+ bSv (5)

After the W matrix is generated the method runs the FastMarching algorithm to calculate the best trajectory, with theFast Marching Method the path found will be the less timepath, in the normal Fast Marching evolution this path will bethe shortest because all the points in the surface will have thesame ”speed” for the front propagation, with the W matrixthe proposed method changes that ”speed” and the trajectorywill be modified depending on the surface conditions andcharacteristics and according to the robot limitations.

As the method is modifying the ”speed” of the FastMarching wave what it does is not only generating a newand different trajectory, it is also giving us a control signal,the speed, and with these speed we can control the robot.

V. ALGORITHM SIMULATIONS

The first simulation we did was to test algorithm to see ifit will work with the W matrix, the matrix was created inrelation to the height (the z coordinate) of the surface, if thez value is 0 the value in the W matrix will be 0 and if the zvalue is the maximum height of the surface the W value willbe 255. The Figure 5 shows the trajectory generates with thenormal method, and in the Figure 6 the trajectory generatedapplying theW matrix. The Figure 7 shows the Fast Marchingpropagation for the normal method and you can appreciate thatthe wave speed is equal for all the point is the surface, but inthe Figure 8 you can tell that the speed is very different, in thepoint where the speed is big we can see the color of the wavefront expands faster and that is why the color band is bigger,

Fig. 5. Geodesic calculated with the normal Fast Marching Method

Fig. 6. Calculated path applying the W matrix

and where the speed is limited the wave travels slower, hencecover less area over the surface, that is why the color band isthin.

After seeing that the algorithm was ready and working thenext steep was to try it with the real W matrix includingthe saturated gradient and the spherical variance, we ran thealgorithm many times to see if it will work in all the cases andgiving different value to the weight values in the equation 5,Figure 9 shows the trajectory that apparently fits the path thata human being will follow, if we change a and b values thepath is changed and will fit the physical characteristics of therobot, in Figure 10 we can see the path variation. The valuesin the equation 5 can be changed if the robot that is going tobe used is different or modified.

Fig. 7. Fast Marching Propagation Wave

Fig. 8. Modified Fast Marching Propagation Wave

Fig. 9. Variations of the Calculated Path Changing the Values in the WMatrix

The trajectories calculated are a tentative path for the robot,the path can be modified on-line modeling the environmentwith the robot sensors and recalculating the trajectory in alocal area.

The last test for the proposed method was considered by usthe last test to verify that the algorithm mas working the waywe wanted, we build a cone surface and the test was that if thealgorithm was ok, the trajectory calculated has tu be aroundthe cone and not a straight line, using the saturated gradientto go from a starting point in the base of the cone to a higherpoint, we wanted that that point was at the top of the cone,but the top is a singular point and the gradient doesn’t existin these point, so we had to move it to a lower point in order

Fig. 10. Fast Marching Propagation Wave

Fig. 11. Path found with gradient restrictions over a 3D cone

Fig. 12. Modified Fast Marching Propagation Wave

to get the algorithm working, in the Figure 11 we see thatthe calculated path was the expected and these simulation isthe proof that these method will really work when the robotwill try to climb a mountain. In the Figure 12 we can see thevariation in the speed of the Fast Marching wave.

All the mentioned tests where made over 21/2D to show thealgorithm behavior over real surfaces, in order to demonstratethat the algorithm really works on fully 3D surfaces the testswhere made over a 3D object, due to the difficulty of finding areal 3D map or a building suitable for the algorithm. The fileformat used for the 3D objects is .off (Object File Format), andthe object is shown in Figure 13, these is a 3D closed objectand the trajectories simulated are clearly 3D paths Figures 1415 16.

The trajectory depends on the wave front propagation, thesespeed depends on the W matrix values, the different speeds forthe wave front are shown in Figures 17 18 19. The zones wherethe color gap is wider means that the front propagation speedin that zone is bigger. It can be seen that the path in Figure 15is shorter that the one in Figure 16, but the trajectory generatedin Figure 16 is softer an may be easier to follow by a robotthat the trajectory un Figure 15. In these simulations where nottaken into account the Robot characteristics and limitations in

Fig. 13. 3D Test Object

Fig. 14. 3D trajectory without using the W matrix

order to apply these method for Climbing Robots, in theoryclimbing a wall shouldn’t be a problem.

A. Environment Reconstruction with the Laser (LRF) Data

A Laser system is basically an active sensor that points amodulated laser radiation to a surface, the beam is reflectedby the surface and these reflection is captured by the sensorthrough an optical receiver located next to the emitter [?],and the Laser system (laser range-finders LRF) mesures thedistance between the sensor and the surface.

As the LRF is mounted over a motorized base in orderto capture 3D data, so there are three variables that need tobe taken into account, ρ that determines the lineal distancefrom the sensor to the surface, φ and θ are the verticaland horizontal angles respectively, a coordinate conversionis needed in order to get the x, y and z coordinates toreconstruct the environment. In the equation 6 is the coordinatetransformation.

Fig. 15. 3D Trajectory using a W matrix generated with G and Sv

Fig. 16. 3D Trajectory changing the a and b values for G and Sv

Fig. 17. Front propagation without W

Fig. 18. Front propagation using W to see the wave front speed changes

Fig. 19. Front propagation with a different W to see the wave front changes

x = ρ ∙ sin θy = ρ cosφ ∙ cos θz = ρ ∙ sinφ ∙ cos θ

(6)

In the equation 6 can be seen that there is a non linearitybetween the two coordinate systems, this means that thecartesian map will be more dispersed in the farthest pointsof the sensor.

In Figure 20 is shown how the environment look like aftermaking the coordinate conversion, with the Laser data there isone very important thing that has to be taken into account, thatis the visibility i.e. if there is a tree in the middle of the surface,the Laser will not be able to see what is behind the tree, so allthe surface behind the tree has to be a non accesible region onthe generated map, and that map is called visibility map, thevisibility map is shown in Figure 21, to build this map it isnecessary to assign the value in the z coordinate of the objectto the cells behind that object. In the Figure 22 is shown the

Fig. 20. 3D Environment Builded from the Laser Data

Fig. 21. Visibility Map Constructed from the Laser Data

trajectory calculated over the visibility map builded from theLaser data, this visibility map will be the local surface that isgoing to replace its area in the global map.

We present an example of the environment modeled withthe laser data, and a trajectory calculated with these method,the Figures 22 and 23 shows the trajectory calculated and theFast Marching wave expansion.

Based on this Laser reconstruction the proposed method isable to refresh the global map and replace old information withthe new local information, and it allows the method tho modifythe previously calculated trajectory and modify the controlsignal to make the robot change its speed and orientation.

Fig. 22. Trajectory Calculated from the Laser Data

Fig. 23. Fast Marching Propagation Wave over the Laser Data

VI. CONCLUSION

The algorithm we presented is a new way to calculatetrajectories for moving a robot over a 3D surface.

One main point about the proposed method is that it can beused not only as a Path Planning Method, it can be used tocontrol the Robot speed to keep it within a range given by thelimit speed allowed over the 3D surface, there are tow valuesthat can be attained from the result of the algorithm, the speedand the robot orientation, the speed is taken from the potentialsurface and the orientation can be taken from the next pointin the trajectory that is going to be occupied by the robot. Ifthe robot’s orientation and the next point where the robot isgoing to be are known, we can calculate the control that hasto be given to the robot in order to make the robot reach thatnext point.

The most important thing about this algorithm is that itworks in real time, it is really fast and give us the possibilityto use it on-line to make decisions with the robot to avoidfixed or moving obstacles.

REFERENCES

[1] J.-C. Latombe, Robot motion planning. Dordrecht, Netherlands: KluwerAcademic Publishers, 1991.

[2] S. Garrido, L. Moreno, and D. Blanco, “Exploration of a cluttered en-vironment using voronoi transform and fast marching method,” Roboticsand Autonomous Systems, vol. 56(12), pp. 1069–1081, 2008.

[3] S. Garrido, L. Moreno, M. Abderrahim, and D. Blanco, “Fm2: A real-time sensor-based feedback controller for mobile robots.” InternationalJournal of Robotics and Automation, vol. Vol. 24. No. 1., pp. 3169–3192,2009.

[4] J. Sethian, Level set methods. Cambridge University Press, 1996.[5] D. T. Lee and B. J. Schachter, “Two algorithms for constructing a

delaunay triangulation,” Int. J. Computer Information Sci., vol. 9, pp.219–242, 1980.

[6] A. Okabe, B. Boots, and K. Sugihara, Spatial Tessellations: Concepts andApplications of Voronoi Diagrams. New York: Wiley, 1992.

[7] K. Mardia and P. Jupp, Directional Statistics. Wiley Series in Probabilityand Statistics, 1999.

[8] D. Carmer and L. Peterson, “Laser radar in robotics,” In Proceedings ofthe IEEE, vol. 84, pp. 299–320, 1996.