Path Planning Strategies for UAVS in 3D Environments

J Intell Robot SystDOI 10.1007/s10846-011-9568-2

Path Planning Strategies for UAVS in 3D Environments

Luca De Filippis · Giorgio Guglieri ·Fulvia Quagliotti

Received: 9 February 2011 / Accepted: 13 April 2011© Springer Science+Business Media B.V. 2011

Abstract The graph-search algorithms developedbetween 60s and 80s were widely used in manyfields, from robotics to video games. The A*algorithm shall be mentioned between some ofthe most important solutions explicitly orientedto motion-robotics, improving the logic of graphsearch with heuristic principles inside the loop.Nevertheless, one of the most important draw-backs of the A* algorithm resides in the headingconstraints connected with the grid characteristics.Different solutions were developed in the lastyears to cope with this problem, based on post-processing algorithms or on improvements of thegraph-search algorithm itself. A very importantone is Theta* that refines the graph search al-lowing to obtain paths with “any” heading. Inthe last two years, the Flight Mechanics ResearchGroup of Politecnico di Torino studied and im-plemented different path planning algorithms. A

L. De Filippis (B) · G. Guglieri · F. QuagliottiDipartimento di Ingegneria Aeronautica e Spaziale,Politecnico di Torino, Corso Duca Degli Abruzzi 24,10129 Turin, Italye-mail: [email protected]

G. Guglierie-mail: [email protected]

F. Quagliottie-mail: [email protected]

Matlab based planning tool was developed, col-lecting four separate approaches: geometric pre-defined trajectories, manual waypoint definition,automatic waypoint distribution (i.e. optimizingcamera payload capabilities) and a comprehen-sive A*-based algorithm used to generate paths,minimizing risk of collision with orographic ob-stacles. The tool named PCube exploits DigitalElevation Maps (DEMs) to assess the risk mapsand it can be used to generate waypoint se-quences for UAVs autopilots. In order to improvethe A*-based algorithm, the solution is extendedto tri-dimensional environments implementing amore effective graph search (based on Theta*). Inthis paper the application of basic Theta* to tri-dimensional path planning will be presented. Par-ticularly, the algorithm is applied to orographicobstacles and in urban environments, to eval-uate the solution for different kinds of obsta-cles. Finally, a comparison with the A* algorithmwill be introduced as a metric of the algorithmperformances.

Keywords Path planning · A* · Theta* ·UAVs · 3D environments

1 Introduction

Path planning has been one of the most importantelements of mission definition and management of

J Intell Robot Syst

manned flight vehicles and it became crucial afterbirth and growth of Unmanned Aerial Vehicles(UAVs), frequently exploiting autonomous flightcapabilities. Mission tasks, mission constraints andplatform characteristics drive the mission manage-ment system and path planning is subject to thesame constraints, being part of the loop. As amatter of fact, the path planning strategy is chosento improve computational time and effectivenessof the system and has to be combined with theother elements in order to comply with the mis-sion requirements.

Generally, path planning aims to generate areal-time trajectory to a target, avoiding obstaclesor collisions (assuming reference flight-conditionsand providing maps of the environment), but alsooptimizing a given functional under kinematicand/or dynamic constraints. Universities, researchcentres and industries developed solutions fordifferent planning requirements: performancesoptimization, collision avoidance, real-time plan-ning or risk minimization. Several algorithms weredeveloped for robotic systems and ground vehi-cles. They took hints from research fields likephysics for potential field algorithms [2, 21], math-ematics for probabilistic approaches [17], or com-puter science for graph search algorithms [11].Each family of algorithms was also tailored forthe path planning of UAVs, and future work willenforce the development of new strategies.

Nevertheless, in order to implement an effec-tive path planning strategy, a deep analysis ofvarious contributing elements is needed. Missiontasks, required payload and surveillance systemsdrive the platform choice, but platform character-istics strongly influence the path. As an example,quad-rotors kinematics gives hovering capabilitiesto these platforms. This feature permits to re-lax turning constraints on the path (which repre-sents a crucial problem for fixed-wing vehicles).The type of mission defines the environment forplanning actions, the path constraints (mountains,hills, valleys, ...) and the required optimizationprocess. The need for off-line or real-time re-planning may also substantially revise the pathplanning strategy for the selected type of missions.Finally, the computational performances of theRemote Control Station (RCS), where the mis-sion management system is generally running, can

influence the algorithm selection and design, astime constraints can be a serious operational issue.

Graph search algorithms were developed forcomputer science to find the shortest path be-tween two nodes of connected graphs. They weredesigned for computer networks to develop rout-ing protocols and were applied to path planningthrough decomposition of the path in waypointsequences. The optimization logic behind these al-gorithms attains the minimization of the distancecovered by the vehicle, but none of its perfor-mances or kinematic characteristics is optimizedalong the path. After the discretization of theenvironment, these algorithms threat each cell ofthe mesh as a graph node and search the shortestpath with a “greedy” logic: the path is obtainedstep-by-step and only a reduced number of cellsare analysed to define the following steps. As amatter of fact, these algorithms don’t try to findthe real optimum path but generate a subopti-mal solution in order to reduce computationaltime. The first positive feature of these solversis their simplicity, which implies reduced com-putational time. Hence, these algorithms can beintegrated in complex mission management sys-tems for multitask platforms or vehicles. Thelow computational requirements are strongly con-nected with the re-planning capabilities. Exploit-ing graph search algorithms, the path can be up-dated along the mission, taking into account themodification or evolution of physical constraints.Therefore, re-planning gives road to different ap-plication fields, like the coordination of multipleagents (formations and even swarms as an ex-ample). For this kind of application the task ofeach agent shall be assigned synchronizing withthe others, in order to cover efficiently the missiontasks and the surveillance area. The paths can bereassigned during the mission, according to theoverall coordination, so re-planning must be fastand effective. As mentioned before, graph searchalgorithms generate the path neglecting vehicle’scharacteristics. This approach allows planning ofthe path for any moving or flying vehicle, butdoesn’t guarantees match of the path with turnand climb limitations. This drawback should befaced using smoothing algorithms to adapt locallythe path to the platform characteristics throughsingle waypoint reallocation, but more effective

J Intell Robot Syst

solutions where developed introducing the plat-form characteristics inside the path planning loop.

With the potential field algorithms the envi-ronment is modelled to generate attractive forcestoward the goal and repulsive ones around theobstacles. The vehicle motion is forced to followthe energy minimum respecting some dynamicconstraints connected with the platform charac-teristics [21]. As a matter of fact these algorithmsgive smoothed and flyable paths, avoiding staticand dynamic obstacles according with the fieldcomplexity. In the last 20 years they have beenwidely investigated and interesting applicationshave been published [12]. Even tough they are apromising solution for path planning and collisionavoidance their application to the here-presentedproblem seemed hard due to their tendency tolocal minima on complex environments.

The use of evolutionary algorithms for vehiclespath optimization is another important solutionpermitting to apply kinematic constraints to thepath. Using Splines or random threes to modelthe trajectory, these algorithms can reallocate thewaypoint sequence to generate optimum solu-tions under constraints on complex environments[10, 16]. Being interesting and flexible, the evo-lutionary algorithms are spreading on differentplanning problems, but their solving complexityis paid with a heavy computational effort [3].Looking for an algorithm to manage complex en-vironments and large amount of data on smallRCSs, the choice of graph-search algorithm forpath planning, together with local optimizationalgorithm to tailor the path on the platform char-acteristics looked reasonable. Also, graph searchalgorithms are a powerful help for planning oflong-term path, where local kinematic constraintsreduce their penalization increasing the distancebetween successive waypoints (as grid spacing ofthe solution is at least one order of magnitudelarger than turn or climb radius).

A Matlab based planning tool was developed,assembling four methods: geometric predefinedtrajectories, manual waypoint definition, auto-matic waypoint distribution (i.e. optimizing opti-cal payload capabilities) [5] and a comprehensiveA*-based algorithm to generate paths, minimiz-ing risk of collision with orographic obstacles [4].The tool named PCube exploits Digital Elevation

Maps (DEMs) to obtain the risk map. It can beused to generate waypoint sequences for UAVs ina format compatible with commercial autopilots.The A*-based algorithm was improved, applyingthe solver to tri-dimensional environments andthen implementing a more effective graph searchsolution: the Theta* algorithm. The primary pur-pose was generating waypoint sequences ready tobe uploaded to commercial autopilots for microand mini fixed-wing UAVs, mainly operating inalpine areas. In this context, the graph search algo-rithms resulted to be the best solution as they areable to generate paths on wide sectional graphswith sustainable computational time. Moreover,small scale UAVs are often linked with com-pact and man-portable RCSs. These ground seg-ments have reduced computational capabilities.Finally, the distribution of orographic-obstacles isre-constructed starting from DEMs mapping ofthe territory. A discrete domain is obtained andthe interpretation of these maps as graphs resultsto be the simplest solution.

From late 50s wide research activity was per-formed on graph-search algorithms within com-puter science, trying to support the design ofcomputer networks. Soon after, the possibility oftheir application in robotics resulted evident andnew solutions were developed to implement al-gorithms tailored for autonomous agents. As aconsequence, research on graph-search methodsbrought new solutions and still continues nowa-days. Therefore, an accurate analysis is requiredto understand advantages and drawbacks of eachproposed approach, in order to find possibleapplication-oriented improvements.

It is well known that a graph is made ofnodes connected with arcs that can be directedor undirected. In graph search, a cost is con-nected with the motion along each arc. Thesealgorithms analyse a given number of nodes sur-rounding the actual position to evaluate the bestsuccessive step in terms of movement cost. TheDijkstra algorithm is one of the first greedy al-gorithms for graph search and permits to findthe minimum path between two nodes of a graphwith positive arc costs [6]. An evolution of theDijkstra algorithm is the Bellman-Ford algorithm[1, 9]; this method finds the minimum path on ori-ented graphs with positive, but also negative costs.

J Intell Robot Syst

Another important method is the Floyd-Warshallalgorithm [8, 20], that finds the shortest pathon a weighted graph with positive and negativeweights, but it reduces the number of evaluatednodes compared with the Dijkstra algorithm. TheA* algorithm is one of the most important solversdeveloped between 50s and 70s, explicitly ori-ented to motion-robotics. A* improved the logicof graph search with heuristic evaluations insidethe loop [11]. Together with the evaluation of thedistance between the current node and the neigh-bours, it also considers the distance between theneighbours and the target end node, as balance forthe estimation of the following steps.

The graph-search algorithms developed be-tween 1960s and 1980s were widely used in manyfields, from robotics to video games, assumingknown deterministic positions of the obstacles onthe map. This was a logic assumption for manyplanning problems, but represented a limit whenrobots moved in unknown environments. Thisproblem excited research on algorithms able toface with map modifications during the path exe-cution. Particularly, results on sensing robots, ableto detect obstacles along the path, induced re-search on algorithms used to re-plan the trajectorywith a more effective strategy than static solverswere able to do. Dynamic re-planning with graphsearch algorithms was introduced. D* (DynamicA*) was published in 1993 and it representedthe evolution of A* for re-planning [18]. Whenchanges occur in the nodes of the graph, only thenew costs of the nodes are updated, exploiting theprevious path. D* expands (the expansion of anode is the analysis of its neighbours to evaluatethe cost of motion from the current node to theneighbour) less nodes than A* because it hasnot to re-plan the whole path through the end.D* focused was the evolution of D*, publishedby the same authors and developed to improveits characteristics [19]. This algorithm improvedthe expansion, reducing the amount of analysednodes and the computational time. Then, researchon dynamic re-planning brought to the develop-ment of Lifelong Planning A* (LPA*) and D*Lite. They are based on the same principles ofD* and D* focused, but they recall the heuristicaspect of A* to improve the speed of the searchprocess [13, 14]. They are very similar and can

be described together. LPA* and D* Lite exploitan incremental search method to update modifiednodes, recalculating only the start distances (i.e.distance from the start cell) that have changed orhave not been calculated before. These algorithmsexploit the change of consistency of the path toreplan.

A* evaluates iteratively the moving cost fromthe current cell to one of its neighbours througha defined cost function. This function (F) is ob-tained summing up two terms:

• H proportional to the heuristic-estimate dis-tance from the evaluated cell to the goal.

• G proportional to the distance from the cur-rent cell to the evaluated one.

The G-value is 0 for the starting cell and it in-creases while the algorithm expands successivecells (i.e. at each step the algorithm sum to themoving cost from the starting cell to the currentone, the distance from the current cell to one ofits neighbours).

To enforce convergence the H-value has to beadmissible and the H-function has to be monotoneor consistent. In other words, at each step the H-value of a cell has not to overestimate the eval-uated distance from the goal and H has to varyalong the path in such a way that:

H (N) ≤ H (C) + G

where:

H(N) heuristic distance from the evaluated cellto the goal;

H(C) heuristic distance from the current cell tothe goal;

G distance from the current cell to the eval-uated one.

When nodes are updated, their G-values canchange. The algorithm records the G-value of thepreceding nodes (the predecessors) and the valueof the updated nodes (the new nodes), comparingthem to verify consistency. The change in consis-tency of the path drives the algorithm search.

Dynamic algorithms allowed new applicationsof graph search methods to path planning of ro-botic systems. More recently, other drawbacks

J Intell Robot Syst

and possible improvements were discovered. Par-ticularly, one of the most important drawbacksof the A* algorithm resides on the heading con-straints connected with the grid characteristics.The graph obtained from a surface map is a meshof eight-connected nodes with undirected arcs.Moving from the current node of the graph to thenext means to move the vehicle from a position toanother one. Considering two nodes of the graphand connecting them with a straight line (i.e. therearen’t obstacles between them), if the slope of theline a is different from

a �= n · π

40 ≤ n ≤ +∞ n ∈ N

it is found that the A* algorithm is not able tofind the real shortest path between the nodes(the straight line itself). A* generates solutionsstrongly suboptimal because of this limit, whichcomes out in any application to path planning.Suboptimal solutions are paths with continuousheading changes and useless vehicle steering (in-creasing control losses) that require some kindof post processing to become feasible. Differentapproaches were developed to cope with thisproblem, based on post-processing algorithms oron improvements of the graph-search algorithmitself. Very important examples are Field D* [7]and Theta* [15]. These algorithms refined thegraph search obtaining generalized paths with“any” heading.

To exploit Field D*, the map must be meshedwith cells of given geometry and the algorithmpropagates information along the edges of thecells. Field D* evaluates neighbours of the currentcell like D*, but it also considers any path fromthe cell to any point along the perimeter of theneighbour. A functional defines the point on theperimeter characterizing the shortest path. Withthis method a wider range of headings can beachieved and shortest paths are obtained. It isknown that graph search algorithms choose persteps from a node to the next. Defining parent theprevious node, just left to arrive at the currentposition, and neighbours the nodes evaluated forthe successive step, Theta* evaluates the distancefrom the parent to one of the neighbours for thecurrent cell so that the shortest path is obtained.

When the algorithm expands the search, it eval-uates two types of paths: from the current nodeto the neighbour (like in A*) and from the parentof the current node to the neighbour. As a con-clusion, paths obtained by the Theta* solver aresmoother and shorter than those generated by A*.

Apparently, Theta* is the most promising so-lution for the path planning of fixed-wing UAVs,used for ground monitoring and surveillance. Asa matter of fact, other graph search algorithmswere not considered due to their primitive con-cepts and considering that the computationalperformances of current digital computers haveovercome many implementation issues. Dynamicalgorithms do not match the present application,being addressed for ground robots equipped withsensing and embedded re-planning capabilities.Furthermore, the application of these algorithmsfor static path planning is less effective than us-ing an advanced static solver [15]. Consideringlong-range flights over highlands and alpine ar-eas, it is assumed that we do not need real-timere-planning, as the map is invariable during themission. Furthermore, using DEMs as a basis toevaluate orographic obstacles, implies that build-ing the graph with nodes instead of cells is easierand therefore Theta* becomes the best approachto the solution.

In this paper the application of a basic ver-sion of Theta* to tri-dimensional path planningis presented. The algorithm is applied both toorographic obstacles and to urban environments,in order to evaluate its responsiveness to differentkinds of obstacles. Finally, comparison with theA* algorithm is presented to outline the advan-tages of this solution method for path planning intri-dimensional environments.

2 Algorithm Description

2.1 Modelling of the Tri-DimensionalEnvironment

Two subroutines were developed to elabo-rate maps and to produce flight paths in tri-dimensional environments, either for orographicobstacles or for urban environments.

J Intell Robot Syst

The first subroutine exploits a DEM map todefine the altitude of each node (mesh) and an im-age to present the area view to the user. As a mat-ter of fact, two types of maps are loaded: the firstis a geo-referenced graphic representation of theenvironment (Fig. 1) and the second is the DEMmap of the same area. The two maps are matchedto obtain the tri-dimensional representation andto build the mesh of the graph (Fig. 1). Start andtarget nodes can be assigned using contours on thegeotiff (Fig. 1) or they can be given in externalinput text-files.

DEM maps are text-files listing latitude, longi-tude and elevation of a given number of pointscomposing the map. These points are matchedwith pixels of the geotiff image, setting the widthand height of the map equal to the number ofhorizontal and vertical points. The elevation ofeach point on the map (being equally spaced inthe horizontal and vertical direction according tothe map resolution) is also included in the DEMdigital formats. We define the environment matrixas the search graph. This graph is a tri-dimensionalmatrix with a number of columns and rows equalto the width and height of the DEM file. Hence,columns and rows of the environment matrix arerelated with longitude and latitude (x and y axes).The third dimension (z axis) of the environment-matrix is defined according to the altitude of thearea, the flight level limitations and the verticalrates (climbing speed). These are the only air-craft characteristics included in the path-planningalgorithm.

Vertical and horizontal spacing are fixed bythe resolution of DEM files and they define theminimum distance between successive nodes. Thisis considered the minimum distance covered bythe aircraft moving from a node to the next withconstant airspeed. Resolution along the third di-mension is assigned according to the nominal ver-tical speed of the vehicle:⎧⎪⎨

⎪⎩

�h = �x · tan γ

γ = arcsin(

RCV

)

where:

�h altitude spacing,�x = �y horizontal and vertical spacing,

a

b

c

Fig. 1 a Graphic representation of the environment.b DEM tri-dimensional representation. c Contour graph

γ climb angle,RC rate of climb,V flight speed.

J Intell Robot Syst

Using the rate of climb to assign the spacing alongthe third dimension of the environment matrixguarantees the feasibility of climbs and fixes thealtitude resolution (i.e. the number of cells alongthe altitude above the ground level):

N3 = hmax − hmin

�h

where:

N3 number of cells along the third dimensionof the environment matrix.

hmax maximum DEM altitude,hmin minimum DEM altitude,

For a given row and column of the matrix(latitude and longitude of a given position), valuesalong the third dimensions are different from zerobelow the altitude given in the DEM file andthey are equal to zero from this altitude to theupper flight limit. To reduce the computationalrequirements, the user may define or restrict thealtitude range of the environment matrix. Withthis option, the lower limit is fixed to the minimumbetween the altitude of the start and the targetnode, while the upper limit is fixed summinga margin to the maximum altitude between thesame nodes.

A simple graphical user interface was designedfor the urban environments, able to define the size

Fig. 2 a Urbanenvironments GUI.b Urban environmentstri-dimensionalrepresentation

a

b

J Intell Robot Syst

of the map and to draw the obstacles interactively.The obstacles are represented as parallelepipedsor cubes and the interface is used to assign theirdimensions and position on the map (Fig. 2).

Once the obstacles are drawn on the map thesequence used to generate the environment ma-trix is the same, but the lower altitude limit is set tozero and the upper limit is defined with a marginadded up to the altitude of the larger obstacle.The model of urban obstacles is very simple, butit is useful to test the algorithm with maps repro-ducing the characteristics typical of these clutteredenvironments.

2.2 Theta* and the Minimum Path Search

The description of the Theta* algorithm is pre-sented in Ref. [15], but it is useful to outline theadaptation of the algorithm to path planning of afixed-wing UAV in a tri-dimensional environment.

The choice of Basic Theta* (first release laterupdated with following versions) is due to theresults published by the authors [15] comparingvarious algorithms and giving estimates for thecomputational load, number of heading changesand path length. Another factor is also the struc-ture of the graph, made of nodes instead of cells.This feature makes the use of the successive ver-sions of the algorithm more complicated, beingmainly addressed for cell-structured graphs.

The first subroutine that needs to be describedis called mask. This subroutine was designed tochoose the neighbours of a node being expanded.If we consider a cube of 26 nodes around the

New nodes

Unfeasible nodes

Obstacles

Nodes on the closed list

Nodes on the open list

Current node

Fig. 3 First strategy for LOS verification

1

2

3

4

5

6

1 2 3 4 5 6Neighbour

Parent

Δr

Fig. 4 First strategy for LOS verification

current one (Fig. 3), mask shall avoid unfeasiblenodes (i.e. nodes out of the mesh limits or nodesrequiring unfeasible trajectories), nodes includ-ing obstacles and nodes included into the closedlist.

For the sake of clarity it’s useful to recall thedefinition of open and close lists in graph searchalgorithms. The open list collects the nodes ex-panded along the graph search. At each step thealgorithm evaluates the nodes surrounding thecurrent one putting them into the open list andsorting the list with respect to the cost-function

Neighbour

Parent

X

Y

Fig. 5 Second strategy for LOS verification

J Intell Robot Syst

Fig. 6 Geometric ambiguity between nodes

value. The first element of the sorted list is movedin the closed list. This list contains the best neigh-bour of the node expanded at each step. Thesenodes are removed from the open list and neverevaluated again and the path is build with nodescoming from this list.

Theta* calls mask for each expanded nodealong the path search. Therefore, its runtimestrongly influences the overall computationaltime. First implementations used a tri-dimensionalmatrix, the OPCL matrix, to assign the status ofeach node (i.e. to record if one node was insidethe open or the closed list) and the evaluation ofthe nodes listed in the closed list was not includedin mask (i.e. avoiding to load the OPCL matrix).Mask was used only to avoid unfeasible nodes

Table 1 Convergencetests for different gainfactors

Test α β Processing time [s] α/β

001 0.01 0.01 No convergence 1.0002 0.01 0.05 No convergence 5.0003 0.001 0.005 No convergence 5.0004 0.01 0.07 No convergence 7.0005 0.001 0.007 No convergence 7.0006 0.01 0.08 No convergence 8.0007 0.001 0.008 No convergence 8.0008 0.01 0.09 13.0 9.0009 0.001 0.009 11.4 9.0010 0.01 0.10 4.2 10.0011 0.001 0.01 4.1 10.0012 0.01 0.11 4.4 11.0013 0.001 0.011 3.7 11.0014 0.01 0.15 0.68 15.0015 0.001 0.015 0.66 15.0016 0.0001 0.001 4.00 10.0017 0.00001 0.0001 4.04 10.0018 0.100 1 3.18 10.0020 0.100 1 4.03 10.0021 1.000 10 2.01 10.0024 1.000 10 4.17 10.0025 0.100 1 1.49 10.0029 0.100 1 2.37 10.0030 0.100 1 2.72 10.0032 0.010 0.10 6.1 10.0032 0.010 0.30 6.94 30.0033 0.010 0.40 6.88 40.0034 0.005 0.20 7.07 40.0035 0.002 0.08 7.1 40.0036 0.021 1.00 No convergence 50.0045 0.030 1.50 No convergence 50.0049 0.040 2.40 No convergence 60.0052 0.050 3.00 No convergence 60.0

J Intell Robot Syst

Table 2 Path 1 (characteristics)

Path 1 A* Theta*

Path length (m) 4850 4618Computational time (s) 1.203 1.393Number of heading changes 42 13Number of altitude changes 159 15Number of path points 358 17

(a priori) and nodes including obstacles (scan-ning at each cycle the environment matrix). Then,the environment matrix was extended in orderto indicate also the status of the nodes, assigningthree different states to the matrix elements. Us-ing 1 to define an obstacle, 2 to define a positioninside the open list and 3 to define a position in-side the closed list, it is possible to use the same tri-dimensional matrix to evaluate the obstacles andthe status of a single node. Revising the content ofthe environment matrix reduced the running timeof mask (together with the overall computationaltime).

Another important routine used by Theta*evaluates the line of sight between nodes and iscalled LoS. As it was mentioned before, Theta*evaluates two paths to define movements from anode to the next one. The first is from the currentnode to its neighbour and the second is from the

parent of the current node to the same neighbour.To evaluate the last path the algorithm verifies thepresence of obstacles between the nodes. In otherwords, the algorithm checks that the path betweenthe parent of the current cell and its neighbouris free. If the path is free the two nodes are con-nected by line of sight (LOS). Verify the LOS on amesh made of nodes instead of cells is a problem.The use of nodes gives the possibility to neglectinformation between the nodes and simplify themesh construction, but it has some drawbacks.Without refined details between the nodes, a lineconnecting two of them can pass near the obsta-cles (i.e. interdicted nodes) without touching them(Fig. 4). Therefore, evaluating LOS only on thenodes walked by a line (orange coloured in Fig. 4),if any, can generate paths that cross obstacles.On the other hand, spotting the significant nodesrepresenting obstacles sufficiently near to the lineof sight (green coloured in Fig. 4) on a discretedomain is not easy, particularly if the domain istri-dimensional.

The first strategy implemented to find sig-nificant nodes in LOS was based on the analysisof the nodes along lines parallel to the local path.This means that, starting from the nodes used toverify the LOS, the subroutine starts checking theother nodes crossed by this line (continuous line

Fig. 7 Path 1: 3Drepresentation

J Intell Robot Syst

in Fig. 4) and continues checking the presenceof further nodes on lines that are parallel to theprevious one (dashed lines in Fig. 4) within agiven range (�r). Figure 4 is a bi-dimensionalexample that gives evidence of the sensitivity ofthe algorithm, setting the range in order to eval-uate accurately the LOS. For a tri-dimensionalpath this problem is even more complicate andrequires a different approach. The line connectingparent and neighbour is used as an approxima-tion to evaluate the significant nodes (Fig. 5).Given the line equation and using subscript p forparent coordinates and subscript n for neighbourcoordinates:

y = a + b x where a = ypxn − ynxp

xn − xpand

b = yn − yp

xn − xp

Substituting the x coordinates of nodes betweenparent and neighbour inside the equation androunding results with the lower and higher integer,the orange nodes in Fig. 4.

Figure 5 are obtained. These are the nodeschecked by the LoS subroutine in the bi-dimensional case. The same method is applied tothe tri-dimensional space where two equations areneeded:

y = a1 + b 1x where a1 = ypxn − ynxp

xn − xpand

b 1 = yn − yp

xn − xp

z = a2 + b 2x where a2 = zpxn − znxp

xn − xpand

b 2 = zn − zp

xn − xp

Rounding the results obtained for the two co-ordinates, the LOS is verified also in the tri-dimensional case.

The approach used to verify the line ofsight is heuristic, therefore space coordinates areindependent one from another and their corre-lation looks forced, but it was verified that di-

viding the general condition in mono, bi and tridimensional sub-conditions (according to parentand neighbour coordinates) good solutions areobtained and the line of sight is verified substan-tially everywhere:

•

⎧⎪⎪⎨

⎪⎪⎩

xp �= xn

yp �= yn

zp �= zn

(a)

(b)

Fig. 8 a Path 1: comparison between A* and Theta*(longitude–latitude plane). b Path 1: comparison betweenA* and Theta* (flight altitude)

J Intell Robot Syst

•

⎧⎪⎪⎨

⎪⎪⎩

xp �= xn

yp �= yn

zp = zn

or

⎧⎪⎪⎨

⎪⎪⎩

xp �= xn

yp = yn

zp �= zn

or

⎧⎪⎪⎨

⎪⎪⎩

xp = xn

yp �= yn

zp �= zn

•

⎧⎪⎪⎨

⎪⎪⎩

xp �= xn

yp = yn

zp = zn

or

⎧⎪⎪⎨

⎪⎪⎩

xp = xn

yp �= yn

zp = zn

or

⎧⎪⎪⎨

⎪⎪⎩

xp = xn

yp = yn

zp �= zn

To complete the description of Theta*, the effectof ambiguity between nodes must be introduced.Ambiguities can be geometrical or functional andarise when two or more nodes have the samecost-function value. Basic Theta* exploits the costfunction of A* to expand the current cell. Thisfunction (F) is made of two terms:

F = α · G + β · H

G moving cost from the current node to theneighbour,

H moving cost (estimated) from the currentnode to the last (target),

α, β gain factors.

In order to describe the generation of ambigu-ities, a strategy to estimate H and G for a givennode in the bi-dimensional case is introduced. Forsimplicity, consider that estimating H is equiv-alent to fixing the cost for each horizontal or

vertical displacement from the node to the targetand multiplying this value for β. Then evaluate G,

to move from the current node to one of the neigh-bours, fixing the cost of an horizontal/vertical dis-placement with respect to a diagonal movement,summing it to the G-value of the current node andmultiplying the result for α. Otherwise evaluateG measuring the distance between the parent ofthe current node to one of its neighbours with thesame method used to evaluate H and multiplyingthe value for α. Figure 6 shows an example ofgeometrical ambiguity, where the current node isred coloured, the target is orange and the greennodes are two of the eight neighbours. Theseneighbours have the same value of the cost func-tion, having equal distances from the current nodeand the target one. For the cube of nodes in Fig. 6,the third dimension increases the ambiguities andthe number of neighbours with same F-valuegrows up.

Functional ambiguity is a more complex prob-lem and regards possibility to find cells with thesame F-value, being far from each other, withdifferent parents and neighbours. This kind ofambiguity is due to the structure of the cost func-tional, obtained summing up the two componentsH and G independently, both assuming similarvalues and varying similarly. In other words, two

Fig. 9 Path 2: 3Drepresentation

J Intell Robot Syst

cells can have a distance from the target and aG-value combined in such a way to give the sameF-value. As for the geometrical ambiguity, thetri-dimensional structure increases the problem,but functional ambiguities grow substantially us-ing Theta*. The algorithm evaluates the G-valueof two paths: from the current cell to one neigh-bour and from the parent of the current cell tothe same neighbour, increasing the possibility tofind a combination of G and H giving the sameF-value.

(a)

(b)

Fig. 10 a Path 2: comparison between A* and Theta*(longitude–latitude plane). b Path 2: comparison betweenA* and Theta* (flight altitude)

A* and Theta* expand a node evaluating theF-values of parents and placing them in the openlist. Then the algorithm sorts the list and chosesthe cell with the smaller F-value, expands thenode and then places this node in the closed list.If the graph search tends to converge, the al-gorithm meets only geometrical ambiguities and,randomly choosing one of the nodes with sameF-value, solves them automatically. Particularlythe algorithm moves to the closed list the firstnode sorted into the open list according to thesorting strategy. Then continues the expansionconverging to the solution. If the algorithm doesnot tend to converge, it starts to add to the openlist nodes with geometrical but also functionalambiguities. The algorithm expands each nodejumping from a point of the graph to another andadding other nodes with same F-value. Ambigui-ties increase and, if the graph is wide (like manytri-dimensional graphs), the algorithm is not ableconverge.

A first strategy to reduce the ambiguities re-sides in a careful choice of the gain factors insidethe cost function. As a matter of fact the choice ofα andβ permits to separate the effects of G and Hover F, strongly reducing the loss of convergence.Tests with different gain factors, applying the al-gorithm to various maps were conducted and thebest ratio between the two gains was fixed to:

α

β= 1

10

A set of tests using different gain ratios is re-ported in Table 1. The results are obtained for theassigned map, fixing the start and target nodes.Other tests were also conducted changing theseassignments.

Table 3 Path 2 (characteristics)

Path 2 A* Theta*


J Intell Robot Syst

Table 4 Urban path 1 (characteristics)

Urban path 1 A* Theta*


3 Results

In the chapter paths planned with the A* algo-rithm are compared with the same paths obtainedusing Theta* and its ability of improving the pathwith comparable computational performance isdemonstrated. Path’s smoothing, obstacles sepa-ration and covered distance are the parametersused to evaluate the algorithms. Their applicationto tri-dimensional environments is considered inorder to understand their merits and drawbacks,even adding the vertical degree of freedom.

All the reported paths are obtained with theMATLAB version 7.11.0 (R2010b), running onMacBook Pro with Intel Core 2 Duo (2 ×2.53 GHz), 4 Gb RAM and MAC OS X 10.5.8.

Two paths planned on alpine highlands arereported (Aosta Valley): the first is a mediumdistance path and the second is an orographicobstacle separation. The first path shows the

ability of the algorithm to plan long tracks intri-dimensional environments, while the secondshows the approach to scaled obstacles.

Finally, other two paths generated in urbanenvironments are used to investigate separationfrom obstacles and planning performance in clut-tered environments.

3.1 Orographic Obstacles

Map characteristics:

• Number of points: 141,372.• �lat: 10 m.• �long: 10 m.• �Z: 5 m.• Environment matrix dimensions: 357 × 396 ×

54 (lat × long × Z).

Table 2 collects the characteristics of a mediumrange planning exercise. The environment matrixcontains 7,634,088 nodes and the mean covereddistance is near to 5 km. The path obtained withTheta* is shorter then that obtained with A*,thanks to the strong reductions of heading andaltitude changes. This reduction is the key pointof the path search and filters out a huge num-ber of nodes with their related heading and al-titude changes. The computational time requiredby Theta* is slightly higher, but the improvement

Fig. 11 Urban path 1: 3Drepresentation

J Intell Robot Syst

on the path is winning. Indeed, the Theta*-basedpath is smoother than the A*-based output andit follows slopes and contours more efficiently asshown in Figs. 7 and 8.

The second path crosses a rocky obstacle re-quiring a steep altitude variation. In this case,the impact on altitude changes of Theta* searchmethod is relevant and the path smoothing effectis evident. The environment matrix contains16,727,040 nodes and the mean covered distanceis 3 km.

(a)

(b)

Fig. 12 a Urban path 1: comparison between A* andTheta* (X–Y plane). b Urban path 1: comparison betweenA* and Theta* (flight altitude)


• Number of points: 152,064.• �lat: 10 m.• �long: 10 m.• �Z: 5 m.• Environment matrix dimensions: 384 × 396 ×

110 (lat × long × Z).

Figure 10 gives evidence that the path searchtowards the target follows the local slope of theterrain with small heading changes due to micro-scale mountain peaks. Figures 9 and 10 show thesmoothing effects on the path and show the im-provement in altitude change as already reportedin Table 3.

3.2 Urban Environments

Urban environments face the solution with dis-crete obstacles (designed with sharp edges) setwithin narrow and cluttered environments. Inthe given exercise, the environment matrix has9,990,000 nodes and distances between nodes are�X = �Y = 1 m and �Z = 0.5 m (Table 4). Thefirst path is planned on a map with only one widebuilding in the middle and with the starting andtarget nodes selected to force the path across thebuilding. This is a test for graph search algorithmsexperiencing convergence delays. The algorithmis forced to expand the nodes along directionsfar from the target with consequent dissipation ofcomputational time.

In this case Theta* is more effective then A*in searching for the optimal path reducing thecomputational time. Less heading and altitudechanges are required using Theta* (the distancebetween start ant target nodes is 300 m). As for

Table 5 Urban path 2 (characteristics)

Urban path 2 A* Theta*


J Intell Robot Syst

Fig. 13 Urban path 2: 3Drepresentation

the other cases, the smoothing effect is shown inFigs. 11 and 12.


• Number of points: 90,000.• �X: 1 m.• �Y: 1 m.• �Z: 0.5 m.• Environment matrix dimensions: 300 × 300 ×

111 (X × Y × Z).

The second urban path is obtained reproducingan environment with different kinds of buildings.This field strongly stresses the FOV verification,forcing the algorithm to check the separation frombuildings. Some paths exhibit penetration of thesmaller obstacles, taking up few nodes (difficult todetect).


• Number of points: 90,000.• �X: 1 m.• �Y: 1 m.• �Z: 0.5 m.• Environment matrix dimensions: 300 × 300 ×

111 (X × Y × Z).

The environment matrix dimensions and the meanpath length are the same of the previous case(Table 5). The computational time is substan-tially reduced, together with heading and altitudechanges. Figures 13 and 14 show the paths ob-tained with the two algorithms and outline the ca-pability of the Theta* algorithm to connect pointswith LOS exploiting any heading variation.

(a)

(b)

Fig. 14 a Urban path 2: comparison between A* andTheta* (X–Y plane). b Urban path 2: comparison betweenA* and Theta* (flight altitude)

J Intell Robot Syst

4 Conclusions and Future Works

Implementing Theta* on 3D graphs requires faireffort, struggling with some drawbacks. Currentresults show that reasonable computational timeis required, considering the number of nodes usedin the graphs and the available computers. Then,comparing the two graph search methods, theadvantages of Theta* become evident. This al-gorithm reduces the length of the track avoid-ing a considerable number of nodes, requiringjust a slightly larger computational time than A*.Theta*-generated paths are smooth and useless al-titude changes are avoided. When obstacles blockthe path, Theta* is able to reduce the searchingtime, exploiting a more effective nodal expansionstrategy.

Both algorithms don’t consider vehicle kine-matics as part of the path generation. This isthe main issue for non-holonomic vehicles likefixed-wing UAVs, requiring a smoothing processto reallocate the waypoints sequences in orderto obtain flyable paths. A solution, adopted tosmooth the path according with turning radiusand rate of climb limitations is the use of theDubins curves. This is the current solutionadopted as post-smoother in the path planningtools developed.

Another option, that is attractive for its lowcomputational impact, is the introduction of thekinematic constraints inside the graph search algo-rithm: the Dubins airplane model is implementedas a constraint in the evaluation of the nodes,combined with obstacles separation and commandoptimization.

Future developments aim to implement moreeffective approaches, even for the simulation ofthe sense and avoid case. Safe paths (in termsof separation from the static obstacles distributedon the map) may be obtained introducing thevehicle kinematics inside the waypoints sequenceelaboration by means of a Model Predictive ap-proach, regenerating the output path piecewise.Using a simple model of the aircraft it is possibleto generate an optimal path over a finite timehorizon, minimizing the distance with respect tothe reference path (given by the graph searchalgorithm) while maintaining adequate separationalso from obstacles eventually detected by the

sensors. Within this approach the updated path isalso constrained by the vehicle’s kinematics.

However, within the current analysis, Theta*still resulted the best choice for path planning ongraphs with the above described characteristics(typical of alpine environments cluttered with ob-stacles). Future work on this algorithm aims toimprove the LOS verification and the overcome ofambiguities. The first task is mandatory for appli-cations within urban environments, enforcing therobustness of the solver. The second task requiresa deeper revision of the algorithm. The presenceof ambiguities is strictly connected with the al-gorithm expansion method and with the graph’sstructure.

Acknowledgements This research work is part of theproject SMAT-F1 (Sistema per il Monitoraggio Avan-zato del Territorio—Fase 1) funded by Regione Piemonte(Italy).

References

1. Bellman, R.: On a routing problem. Q. Appl. Math.16(1), 87–90 (1958)

2. Bertuccelli, L.F., How, J.P.: Robust UAV search forenvironmentas with imprecise probability maps. In:IEEE Conference of Decision and Control, Seville,Spain (2005)

3. Capozzi, B.J.: Evolution-based path planning and man-agement for autonomous UAVs. Ph.D. Dissertation,University of Washington, USA (2001)

4. De Filippis, L., Guglieri, G., Quagliotti, F.: A minimumrisk approach for path planning of UAVs. J. Intell.Robot. Syst. 1(2011), 203–222 (2011)

5. De Filippis, L., Guglieri, G., Quagliotti, F.: FlightAnalysis and Design for Mini-UAVs. XX AIDAACongress, Milano, Italy (2009)

6. Dijkstra, E.W.: A note to two problems in connexionwith graphs. Numer. Math. 1, 269–271 (1959)

7. Ferguson, D., Stentz, A.: Using interpolation to im-prove path planning: the field D* algorithm. J. FieldRobot. 23(2), 79–101 (2006)

8. Floyd, R.W.: Algorithm 97: shortest path. Commun.ACM 5(6), 345 (1962)

9. Ford, L.R., Jr., Fulkerson, D.R.: Flows in Networks.Princeton University Press (1962)

10. Guglieri, G., Quagliotti, F., Speciale, G.: Optimal tra-jectory tracking for an autonomous Uav. In: AutomaticControl in Aerospace, vol. 1(1) (2008)

11. Hart, P., Nilsson, N., Raphael, B.: A formal basis forthe heuristic determination of minimum cost paths.IEEE Trans. Syst. Sci. Cybern. SCC-4(2), 100–107(1968)

J Intell Robot Syst

12. Horner, D.P., Healey, A.J.: Use of artificial potentialfields for UAV guidance and optimization of WLANcommunications. In: Autonomous Underwater Vehi-cles, 2004 IEEE/OES, pp. 88–95, 17–18 June 2004

13. Koenig, S., Likhachev, M.: D* Lite. In: Proceedingof the AAAI Conference on Artificial Intelligence,pp. 476–483 (2002)

14. Koenig, S., Likhachev, M.: Incremental A*. In: Pro-ceeding of the Natural Information Processing Systems(2001)

15. Nash, A., Daniel, K., Koenig, S., Felner, A.: Theta*:any-angle path planning on grids. In: Proceedingsof the AAAI Conference on Artificial Intelligence,pp. 1177–1183 (2007)

16. Nikolos, I.K., Tsourveloudis, N.C., Valavanis, K.P.:Evolutionary algorithm based offline/online path plan-ner for UAV navigation. IEEE Trans. Syst. ManCybern., Part B, Cybern. 33(6), 898–912 (2003)

17. Pfeiffer, B., Batta, R., Klamroth, K., Nagi, R.: Pathplanning for UAVs in the presence of threat zonesusing probabilistic modelling. In: Handbook of Mili-tary Industrial Engineering. Taylor and Francis, USA(2008)

18. Stentz, A.: Optimal and efficient path planning for un-known and dynamic environments. Carnegie MellonRobotics Institute Technical Report, CMU-RI-TR-93-20 (1993)

19. Stentz, A.: The focussed D* algorithm for real-timereplanning. In: Proceedings of the International JointConference on Artificial Intelligence, pp. 1652–1659(1995)

20. Warshall, S.: A theorem on Boolean matrices. J. ACM9(1), 11–12 (1962)

21. Waydo, S., Murray, R.M.: Vehicle motion planning us-ing stream functions. In: 2003 IEEE International Con-ference on Robotics and Automation (2003)

Documents

Path Planning Strategies for UAVS in 3D Environments