The 9th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 2012) Nov. 26-28, 2012 in Daejeon Convention Center(DCC), Daejeon, Korea

Global Path Planning on Uneven Elevation Maps Sunglok Choi, Jaehyun Park, Eulgyoon Lim, and Wonpil Yu Robot and Cognitive Convergence Research Dept., ETRI, Republic of Korea (E-mail: [email protected], Web: http://sites.google.com/site/sunglok) Abstract - This paper introduce about graph-search based global path planning on uneven elevation maps. An elevation map is an efficient and popular representation for 3-D terrains due to its easy manipulation by a computer. On the elevation map, we investigate three different optimal paths in the aspects of travel distance, travel time, and energy consumption. A distance/timeoptimal path is derived by simple extension of A* on 2-D grid maps. A formulation on energy consumption leads an energy-optimal version and traversiblity criteria. We demonstrate effectiveness of our proposed method by experiments on randomly generated Gaussian hills. Keywords - Path Planning, Elevation Map, A*, Timeoptimal, Distance-optimal, Energy-optimal

1. Introduction Global path planning is a process to generate a collision-free path from the start to the goal on the given map. It is one of the most fundamental and inevitable tasks for autonomous navigation. Due to such importance, there have been a number of works on global path planning from beginning of robotics. These works mostly solved the path-finding problem by modeling traversable space (or states) as graphs. For example, a visibility graph, Voronoi diagram, state lattice, roadmap, and grid map are kinds of graphs describing connectivity of the given space. A* and their variants are popular and effective tools for searching a path on the given graphs. Sometimes it is difficult to represent the given configuration space (c-space) as a complete and dense graph because the c-space is high-dimensional or continuous with complex constraints. A solution to overcome such difficulty is building a partial and random graph by sampling sufficient number of feasible states on the c-space. Probabilistic roadmap method (PRM) and rapidly-exploring random tree (RRT) are representative examples. The world model (a.k.a. an appearance map) is important for global path planning to understand traversability and connectivity of the given space. The model can be classified by its types and dimension of description format. Appearance of 2-D space is described by a grid or vector map similarly to a raster and vector image in graphics. In robotics, a grid map is favored because it is easy for a computer to access and manipulate (e.g. build and update). Each cell on a grid map can have its value of obstacle occupancy (a.k.a. an occupancy grid map) or traversal cost (a.k.a. a costmap). Some robotic appliThis work was supported partly by the R&D program of MKE and KIAT. (Project: 3-D Perception and Robot Navigation Technology for Unstructured Environments, M002300090)

cations need 3-D world modeling due to 3-D structures such as stairs, bumps, ramps, rough terrains, hills, and valleys. Similarly appearance of 3-D space is represented by a set of voxels or polygons. Sometimes two-and-onehalf-dimensional (2.5-D) world modeling is enough for ground vehicles to depict their 3-D operating spaces. An elevation map is another type of grid maps, whose cells have their value of ground level. Since it has same advantages with a grid map, many recent researches on ground robots have been based on an elevation map. Related Works There have been few researches about global path planning on 2.5-D world models. One of the initial attempts was performed by Gaw and Meystel [1]. They represented the 2.5-D world as polygonalized isolines and found an energy-optimal path considering elevation changes between adjacent isolines. Later, this work was more extended by Rowe [2] and Sun and Reif [3]. Two extended works both modeled the 2.5-D world as a set of polygons and investigated an energy-optimal path by A* and a variant of Dijkstra’s algorithm, respectively. Both also adopted concepts of impermissible heading ranges (due to steep uphill and side-slope overturn) and braking heading ranges (due to steep downhill). As Rowe mentioned, an elevation map may have disadvantages such as quantization error and wasting memory. However, thanks to recent improvements in computing capability, we can easily deal with a densely sampled elevation map. Moreover, similarly to a grid map, an elevation map is also preferred because it is easy to be dealt by a computer. Spero and Javis [4] used RRT on an elevation map, but they only considered about two status of occupancy (free or obstacle) on the elevation map. One of recent researches by Liu and Sun [5] applied A* to an elevation map to generate an energy-optimal path considering friction and energy loss. However, it did not take into account of potential energy by elevation change. Even though there have been several works on path planning on a polygon map, there are few researches on an elevation map. Moreover, works on an elevation map did not completely consider uneven ground level, so we believe that it needs to be investigated more. This paper introduces about global path planning on uneven elevation maps. We follow graph-search based approach on elevation maps and investigate three different optimal paths: travel distance, travel time, and energy consumption. This is our major contribution of this paper compared to related works mentioned earlier. To simplify the problem, we are based on four assumptions on the robot and its operating space. The assumptions are described in Section 2.. In Section 3, at first, we introduce about A* algorithm and its application to 2-D grid

=

= time*

≤

distance

energy

energy

<

time

=

uneven

distance

=

even

Fig. 1: All three optimality criteria are same regardless of the robot’s moving direction on even terrains if the robot has constant velocity. However, on uneven terrains, energy consumption is different with respect to slope and heading direction, that is, anisotropic. Travel time∗ might be also not same if the robot become faster during its descending.

maps. We extend this basic A* to optimal path planning on elevation maps. Distance/time-optimal A* on elevation maps is simply derived from the 2-D version of A* by extending its dimension. Energy-optimal A* is derived based on energy consumption by the robot’s traversal. The energy consumption is calculated through gravity, friction, drag, and energy loss. We also propose a way to check traversability of cells on elevation maps, which is our second contribution. We performed experiments on random Gaussian hills to verify effectiveness of the proposed path planning, which is demonstrated in Section 4.

2. Problem Formulation Our problem is to find an optimal path from the given start to the goal on an uneven elevation map. The optimality is quantified by physically meaningful measures such as travel distance, travel time, and energy consumption. As shown in Figure 1, energy criteria is anisotropic according to slope and heading even though travel distance is same, so an energy-optimal path might be also meaningful in some aspects. An elevation map is a kind of grid maps whose cells additionally have their values of ground level at their location. The value of elevation for a cell n is written by n.z. Assumptions We assume four conditions to simplify the problem. First, the given robot is holonomic so that it can change its heading on a cell without translation. An omni-directional mobile robot satisfies this condition. A differential drive, the most popular one in robotics, also roughly follows it because its minimal radius-ofrotation is zero. Second, we assume that the robot is a point on the given map. This assumption makes the problem free from complex situations such as collision detection and multiple ground contacts. These two conditions are not realistic but common and efficient approximation for many global path planning, especially graph-search based approaches. Third, the robot moves with constant

linear velocity, vc . Since travel distance and time are proportional under this assumption, a distance-optimal path becomes a time-optimal path. Finally, we assume that configuration and parameters of the robot and its operating space are known and constant. In details, the robot has constant mass m. Energy loss may be happened due to energy transfer and internal consumption, whose value per second is written in Pl . Originally it is a nonlinear function on load, velocity, and others [6], but we can roughly regard it as constant because many parameters are fixed. The robot can exhibit the maximal power Pmax and its corresponding maximal driving force will be noted in Fmax . The maximal driving force is also derived by Fmax = (Pmax − Pl )/vc . Moreover, we need to consider three external forces affected on the line of motion, gravity Fg , friction force Ff , and drag force Fd , which are described by Fg (θ ) = mg sin θ , Ff (θ ) = µmg cos θ , and

(1) (2)

Fd = 0.5ρv2c cd A ,

(3)

where θ is the inclined angle from the horizontal, g is the gravitational field strength, µ is the friction coefficient, rho is the density of air, cd is the drag coefficient, and A is the cross-sectional area of the robot. Figure 2 presents these forces according to configurations of the robot. Shortly we will write the overall power consumption by gravity, friction, drag, and energy loss as
Pcon (θ ) = Fg (θ ) + Ff (θ ) + Fd vc + Pl . (4) Since the inclined angle θ is positive when the robot comes up and negative when the robot comes down, so the gravitational force parallel to the incline, Fg , always has correct signs regardless of the robot’s moving direction.

3. A* Path Planning on Elevation Maps 3.1 A* Path Planning A* is a graph-search algorithm to find the least-cost path from the start node ns to the goal node ng . From the start node ns , A* sequentially explores a visitable node1 which can probably entail the least-cost path along the node. Such sequential exploration is terminated when A* reaches the goal node ng . A fitness function quantifies the probable degree of being the least-cost path along a node n. It is defined by sum of a cost function g and heuristic function h as follows: f (n) = g(ns , n) + h(n, ng ) .

(5)

A cost function g(ns , n) is (sequentially driven) traveled cost from ns to the query node n, and a heuristic function h(n, ng ) is estimated cost from n to ng . The traveled cost is calculated by accumulating each edge-cost on a path from ns like g(ns , n) = g(ns , p(n)) + c(p(n), n) , 1A

node in OPEN priority queue

(6)

(a) External Force & Sliding (b) Internal Torque & Rolling (c) Trust Force & Hovering Configuration 𝜃

𝜃

𝜃

Max. Driving Force

* 𝐹𝑚𝑎𝑥 = 𝐹𝑒𝑥

𝐹𝑚𝑎𝑥 = 𝜇𝑠 𝑚𝑔 cos 𝜃

* 𝐹𝑚𝑎𝑥 = 𝐹𝑡𝑟

Gravity+Normal Force

𝐹𝑔 = 𝑚𝑔 sin 𝜃

𝐹𝑔 = 𝑚𝑔 sin 𝜃

𝐹𝑔 = 𝑚𝑔

Friction Force

𝐹𝑓 = 𝜇𝑘 𝑚𝑔 cos 𝜃

𝐹𝑓 = 𝜇𝑟 𝑚𝑔 cos 𝜃

𝐹𝑓 = 0 (𝜇 = 0)

Fig. 2: The robot on the ramp is presented in three combinations of maximal driving forces, sum of gravity and normal forces, and friction forces. Fex∗ is the maximal external driving force given by other devices or human. Ftr∗ is the maximal thrust force which is a reaction force by spinning propellers or jet engines. Moreover, it is necessary to use three different friction coefficients corresponding to the given configuration. µk is the kinetic friction coefficient, µs is the static friction coefficient, and µr is the rolling friction coefficient. They usually follows the inequality, µs ≥ µk ≥ µr . These figures do not describe other effective forces such as drag force and lift force because they are mostly same regardless of configurations. where c is an edge-cost from its parent node p(n). The parent node is selected as one of already visited neighborhood nodes2 which can make the child node n have the least traveled cost from ns . To generate the least-cost path, the heuristic should be less than equal to the minimum of all feasible traveled cost from n to ng , which is known as the admissible condition. 3.2 A* Path Planning on 2-D Grid Maps A 2-D grid map is one of the simplest world representations for indoor and mild-terrain outdoor environments. Each cell is located at (n.x, n.y) and contains information on obstacle occupancy or traversal cost. A* and its variants are popularly applied to a 2-D grid map for goal-directed navigation. In the view of graphsearch, non-occupied cells are nodes of a graph, and they are connected to their non-occupied adjacent cells with their respective edge-costs. When all cells have a same value of traversal cost (a.k.a. a uniform costmap), their edge-cost is usually defined by distance like cU (n p , nc ) = d(n p , nc ) where q d(n p , nc ) = (nc .x − n p .x)2 + (nc .y − n p .y)2 .

(7) (8)

Since a child cell and√ its parent are adjacent, the distance is usually δ or 2δ where δ is cell size. If each cell has a different value of traversal cost (a.k.a. a nonuniform costmap), its edge-cost should take into account its traversal cost like n p .t + nc .t cN (n p , nc ) = d(n p , nc ) , (9) 2 where n.t is traversal cost of n, which means the degree of difficulty to move across the cell. One of popular heuristic functions is Euclidean distance, hU (n, ng ) = d(n, ng ) ,

(10)

which satisfy the admissible condition on the flat world. Even on a non-uniform costmap, we assume the expected 2A

node in OPEN or CLOSE list

traversal cost as 1 because it is difficult to estimate the expectation ahead, that is hN (n, ng ) = hU (n, ng ). Moreover, it needs be careful not to violate the admissible condition because it can be broken when traversal cost of cells is less than 1. 3.3 Traversability on 2.5-D Elevation Maps A 2.5-D elevation map is a simple extension of grid map whose cells contain additional information on their ground levels. Due to such augmentation, it is possible to describe uneven terrains such as bumps, ramps, hills, and valleys. A* need to know that it is traversable or not from the parent cell n p to its child nc in order to expand its search. On a 2-D grid map, we can know such traversablility directly from a value of occupancy or traversal cost3 . However, we need to derive the traversability based on the physical constraints on a 2.5-D elevation map. First, the robot can move if the required power becomes less than equal to its maximal power, Pcon ≤ Pmax . In the view of forces, this condition becomes Fg + Ff + Fd ≤ Fmax .

(11)

Second, the robot unintentionally slides down ‘steep’ downhills because its gravity strongly pushes it down. Moreover, the robot may be accelerated during its sliding, which means this situation breaks our third assumption in Section 2. It might be possible for the robot to navigate on the steep downhills by reverse driving forces such as backward spinning wheels and fans. We will not consider this solution because it is sometimes uncontrollable and difficult to consider its complex and nonlinear phenomenon in global path planning. In the conservative aspect, we define this situation as intraversable and lead additional condition like Fg + Ff∗ + Fd > 0 , 3A

(12)

cell is not traversable when it has occupancy value more than the threshold or has an infinite cost value.

Pl / vc = 0 [J/m] Pl / vc = 60 [J/m]

200

Pl / vc = 120 [J/m] 20

150

15

1 Z [m]

Energy Consumption per Distance [J/m]

250

100

0 0

5 5 X [m]

50

0 −30

10

0.5

−20

−10

0 θ [degree]

10

20

10

15

0

Fig. 4: The Gaussian hill has various shapes of terrains originated from multiple Gaussian functions with randomly assigned mean, variance, and height.

30

Fig. 3: Each line shows energy consumption per distance with a different value of energy loss. We assume that the other parameters are same with our experimental configuration, which is described in Section 4.1.

tion 2, the distance-optimal edge-cost is same with the time-optimal edge-cost, cT (n p , nc ) = cD (n p , nc ) ,

where Ff∗ is the maximal friction force, probably static friction. For example, these two conditions become much simpler inequality like −µs < tan θ (n p , nc ) =

∆(n p , nc ) ≤ µs − µr d(n p , nc )

(13)

in the internal torque and rolling configuration shown in Figure 2 (b) with no drag force. This inequality is well matched with our intuition that the robot cannot climb on steep uphills and becomes uncontrollable on severe downhills. This configuration also has a very interesting threshold that the robot needs no driving force, Fg + Ff + Fd = 0. The threshold is at tan θ (n p , nc ) = −µr when there is no drag force. The robot will move with ideal rolling when the slope is more than the threshold. However, it suffers from sliding when the slope is less than the threshold, so the robot needs to hold and release its wheels frequently to keep the constant velocity. Therefore, the friction coefficient is varied like  µk if −µs < tan θ < −µr µ(θ ) = , (14) µr if −µr ≤ tan θ ≤ µs − µr which is briefly shown in Figure 3. 3.4 A* Path Planning on 2.5-D Elevation Maps We apply A* to an elevation map with additional consideration on elevation changes. We follow the basic of A* described in the previous subsections. For a distanceoptimal path, the edge-cost should take into account not only horizontal distance, but also vertical distance. The distance-optimal edge-cost is derived as cD (n p , nc ) = s(n p , nc ) where q s(n p , nc ) = d(n p , nc )2 + ∆(n p , nc )2 ∆(n p , nc ) = nc .z − n p .z ,

(15) and

Y [m]

(16) (17)

which is three-dimensional distance between two cells considering elevation difference ∆. As mentioned in Sec-

(18)

because we assume that the robot has constant linear velocity. Similarly, for an energy-optimal path, the edgecost should be defined by required energy to move between two cells as follows: 
s(n p , nc )  ,0 . (19) cE (n p , nc ) = max Pcon θ (n p , nc ) vc We ignore kinetic energy of the robot in the edge-cost because it is constant all the time due to constant velocity assumption. The edge cost cE is limited to more than equal to 0 since Pcon can be negative when the node nc is at a deep valley, ∆  0. Specifically, in the internal torque and rolling configuration shown in Figure 2 (b) with Fd = 0, it is possible to lead much simpler form like  cE (n p , nc ) = max mg ∆(n p ,nc ) + µ(θ (n p ,nc )) d (n p ,nc )+
 Pcon (θ (n p ,nc )) s(n p ,nc )/vc , 0 . (20) Finally we can define heuristic functions for distance/timeoptimal and energy-optimal paths as follows: hD (n, ng ) = hT (n, ng ) = s(n, ng ) 
s(n, ng )  hE (n, ng ) = max Pcon θ (n, ng ) ,0 . vc

(21) (22)

4. Experiments 4.1 Configuration We perform experiments on simulated environments to verify effectiveness of the proposed path planning. Map We used randomly generated Gaussian hill composed by multiple Gaussian functions with randomly selected mean, variance, and height. An example is shown in Figure 4. Since it can represent diverse appearance of terrains, we can achieve statistically meaningful results regardless of a specific type of terrains. Gaussian hills were generated within 15-by-20 m2 rectangular area. Each hill was composed by 25 Gaussian functions whose

height was randomly distributed within [0.4, 1.2] m. Finally, we made a elevation map from the Gaussian hill by quantization whose horizontal cell size was 0.1 m and horizontal level size was 0.05 m. Robot We assumed that the robot moved vc = 0.5m/s linear velocity with m = 12 kg mass. The robot was driven by internal torque from wheels as shown in Figure 2 (b). The friction coefficients between the ground and wheels are µs = 1.0, µk = 0.8, and µr = 0.01, respectively, which is similar to friction between concrete surfaces and hard rubbers. The robot additionally spent energy around Pl = 30 W due to its energy transfer and internal circuits. Algorithms We executed three algorithms to evaluate their performance: A* on 2-D grid maps, distanceoptimal A* on 2.5-D elevation maps, and energy-optimal A* on 2.5-D elevation maps. We measured their performance based on travel distance, energy consumption, and computing time. We implemented three algorithms in C++ and executed them in Intel Core i7 at 2.80 GHz (using only single core) with 8 GB RAM. For statistically meaningful results, we executed path planning 100 times from random starts to random goals on 10 randomly generated maps (overall: 1, 000 times). 4.2 Results The experimental results are described by three measures with respect to path length which is the number of cells composing a path. This representation explains performance more exactly because three measures highly depend on path length. The performance is presented in Figure 5, and path examples are shown in Figure 6. In the sake of convenience, A* on 2-D grid maps, distanceoptimal A*, and energy-optimal distance will be noted by A*-2D, A*-Dopt , and A*-Eopt , respectively. First, three algorithms had almost similar travel distance, and A*-Dopt had slightly better than the others. Since our distance measure is diagonal distance (a.k.a. octile distance), three algorithms had similar distance even though many examples shown in Figure 6 seem quite different in Euclidean sense. However, it is clear that two proposed algorithms generated much natural paths than A*-2D because they avoided unnecessary hill climbing. Second, A*-Eopt found a path which required almost 10 percents less energy than A*-2D. A*-Dopt was also better than A*2D because it went around unnecessary hills. Third, two proposed algorithms spent more time than A*-2D due to more complex calculation of edge-cost. A*-Dopt spent around 30 percents more computing time, and A*-Eopt spent three times more computing time. 4.3 Limitations We found several limitations of the proposed algorithms during experiments. It would be good information for other researchers and further works about path planning on elevation maps. First of all, energy-based path planning on an elevation map may suffer from serious quantization error. For example, in our experimental configuration, each cell has elevation error almost 0.025

𝜃

Fig. 7: Even though the robot cannot climb the steep ramp straightly, it can go up as a series of zigzag movements.

m in average. The error causes much larger error in potential energy around 2.94 J per each cell, and the derived error is sequentially accumulated along a path. If its path length is 100, the accumulated error in potential energy is around 294 J, which is significant as shown in Figure 5 (b). Probably our energy-optimal path is not optimal in real environments. When we measured the required energy of path by A*-Eopt in non-quantized maps, it often worse than a path by A*-Dopt . A polygon map may have a similar problem because it is also an approximation on real environments. Second, it is necessary to relax our four assumptions, especially constant velocity assumption. To satisfy the constant velocity assumption, we assume the robot will rotate and hold its wheels repeatedly on downhills. Since we regard that such motion undergoes kinetic friction, its energy consumption is much more than that of going up. In other words, our energy consumption model shown in Figure 3 is unnatural because of keeping constant velocity. Finally, A* seems not enough for path planning on an elevation map. A* algorithm stretches its search toward only its eight neighborhoods, so it cannot check traversability of cells which are more than two cells apart. For example, there is a ramp which is so steep that the robot cannot climb straightly. A* cannot find an alternative feasible path which is a series of zigzags as shown in Figure 7. We believe that any-angle path planning [7] can solve this problem.

5. Conclusion In this paper, we propose A* path planning on uneven elevation maps. Under four assumptions to simplify the problem, we extend A* to generate distance/time-optimal paths and energy-optimal paths on 2.5-D maps. Moreover, we explain about how to check traversability from elevation information with physical constraints. We also show effectiveness of the proposed path planning through experiments on randomly generated Gaussian hills. As further works, we need to deal with limitations of our current approach, which is described in Section 4.3.

References [1] D. Gaw and A. Meystel, “Minimum-time navigation of an unmanned mobile robot in a 2-1/2d world with obstacles,” in Proceedings of IEEE International

1.5

20

60

A 2D

A* 2D

A* D

A* D

A Eopt

10

5

A* E

Computing Time [msec]

*

15

A* 2D A* D

50

opt

opt

Energy Consumption [kJ]

Travel Distance [m]

*

opt

1

0.5

opt

A* E

opt

40 30 20 10

0 20

40

60 80 100 Path Length [cell]

120

140

0 20

40

(a) Travel Distance

60 80 100 Path Length [cell]

120

140

0 20

40

(b) Energy Consumption

60 80 100 Path Length [cell]

120

140

(c) Computing Time

Fig. 5: The performance of three algorithms is quantified by three measures: (a) travel distance, (b) energy consumption, and (c) computing time. 20

20

20

18

18

18

20

16

16

16

14

14

14

14

12

12

12

12

10

A Dopt A* Eopt

10

18

A* Dopt

16

A* Eopt

10

8

8

8

8

6

6

6

6

4

4

2

2

A* 2D 4

0

*

5

10

A* Dopt *

2

A Eopt 0

A* 2D

4

A* Dopt

2

15

0

A Eopt 0

5

X [m]

10

15

0

0

5

X [m]

(a) 20 A Dopt *

A Eopt

16

0

20 A Dopt *

A Eopt

16

*

18

*

16

A Dopt A Eopt

16

12

12

12 Y [m]

12

Y [m]

14

Y [m]

14

10

10

8

8

8

6

6

6

6

4

4

4

4

2

2

2

2

0

0

0

10 X [m]

(e)

15

0

5

10 X [m]

(f)

15

15

0

5

10 X [m]

(g)

15

10

15

A* 2D A* Dopt A* Eopt

10

8

5

10

20 A* 2D

18

*

14

0

5

(d)

14

10

0

X [m]

A* 2D 18

*

15

(c)

A* 2D 18

10 X [m]

(b)

20

Y [m]

A* 2D

*

Y [m]

10

Y [m]

Y [m]

Y [m]

A* 2D

0

0

5 X [m]

(h)

Fig. 6: Each figure shows an example path generated by three algorithms on randomly generated Gaussian hills. Conference on Robotics and Automation (ICRA), 1986. [2] N. C. Rowe and R. S. Ross, “Optimal grid-free path planning across arbitrarily-contoured terrain with anisotropic friction and gravity effects,” IEEE Transactions on Robotics and Automation, vol. 6, pp. 540– 553, 1990. [3] Z. Sun and J. H. Reif, “On finding energyminimizing paths on terrains,” IEEE Transactions on Robotics (TRO), vol. 21, pp. 102–114, 2005. [4] D. J. Spero and R. A. Javis, “Path planning for a mobile robot in a rough terrain environment,” in Proceedings on International Workshop on Robot Motion and Control (RoMoCo), 2002.

[5] S. Liu and D. Sun, “Optimal motion planning of a mobile robot with minimum energy consumption,” in Proceedings on IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), 2011. [6] Y. Mei, Y. Lu, Y. Hu, and C. Lee, “Deployment of mobile robots with energy and timing constraints,” IEEE Transactions on Robotics (TRO), vol. 22, pp. 507–522, 2006. [7] S. Choi and W. Yu, “Any-angle path planning on non-uniform costmaps,” in Proceedings of IEEE International Conference on Robotics and Automation (ICRA), 2011.