The 7th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 2010)
Theta* Path Planning with Averaged Cost on Non-uniform Cost Maps Sunglok Choi and Wonpil Yu Robot Research Department, ETRI, Daejeon, Republic of Korea {sunglok, ywp}@etri.re.kr
Abstract— A* on a grid map suffers from zig-zag pattern as shown in Figure 1(a). Theta* is the state-of-the-art solution to overcome the problem. However, it does not work on a non-uniform cost map because it calculates a path-cost using Euclidean distance. Euclidean distance assumes that all cellcosts are uniform. In this paper, we extend Theta* to a nonuniform cost map. When Theta* calculates a path-cost between two cells, we compensate the path-cost as much as average of cell-costs on their line-of-sight. We propose two different methods to calculate the averaged cell-cost using arithmetic mean. We also experimentally measured their accuracy. Keywords— Any-angle Path Planning, Field D∗ , Theta∗ , Nonuniform Cost Map
1. Introduction
(a) A*
(b) Theta*
Fig. 1. A path by A* and Theta* on a grid map (c.f. blue: visited cells in open list, cyan: visited cells in close list)
as much as average of intermediate cell-costs on the lineof-sight. As the first method, the average of cell-costs assumes that the intermediate cell-costs contribute the pathcost evenly. However, both sides of line-of-sight obviously contribute the path-cost as much as half of their cell-cost. As the second method, the averaged cell-cost can consider both sides of the line-of-sight. The second method can achieve higher accuracy than the first method. We verify it experimentally.
Path planning is closely related with how to represent the given space. A grid map is popular representation because it is easy to build and modify. Many works including mapping, SLAM, and path planning have utilized the grid map. It represents the space as a collection of discrete and regular cells. It is quite similar with a bitmap image. A* is a popular algorithm in path planning on the grid map. It is faster than sampling-based algorithms such as PRM and RRT. It also generates shorter path than them. Moreover, it can consider non-uniform cell-cost on the grid map. We believe that sampling-based methods are more suitable for higherdimensional space (i.e. motion planning of manipulators), not two-dimensional space (i.e. path planning of holonomic robots). A* generates a path with zig-zag pattern as shown in Figure 1(a). The problem results from a grid map which allows A* to search toward only 8 directions. Meaningful researches have investigated to solve such zig-zag problem. Field D∗ [1] and Theta∗ [2] tackled the heart of the zigzag problem. In contrast to A∗ , their parent cell extend its children toward any direction, not 8 directions. Nash et al. had evaluated Theta∗ compared with other methods including Field D∗ . They reported that Theta∗ was the best performance [2]. However, Theta∗ does not works on a nonuniform cost map. In this paper, we propose two methods to extend Theta* toward a non-uniform cost map. Two methods are inspired from a statistical measure, average, which summarizes overall values approximately. We can compensate a path-cost
2. Theta* Path Planning Revisited Theta* is identical to A* except a way of updating parents. It is described in Algorithm 1, which is same with A*. A pair of cell, cstart and cgoal , is given as the start and goal cells. A set Mempty contains movable cells on a grid map. Theta* terminates its search with a resultant path which represented as a sequence of cells, P = {c1 , c2 , . . . , cn }. Theta* uses Euclidean distance as its heuristic function h. Theta* selects a parent cell according to result of a collision test [3]. The detail is described in Algorithm 2, which is only difference with A*. If there is no occupied cell on the line-of-sight between the parent of cbest and newly expanded cell cchild , The cell cchild shares the same parent with cbest (Algorithm 2: Line 1 – 2). It enables Theta* to expand its search to any direction and any distance. If the collision test always reports collision, Theta* becomes exactly same with A∗ . The overall cost from the parent to the child cchild is calculated by Euclidean distance (Algorithm 2: Line 6). A function d is Euclidean distance between two cells.
This work was supported partly by the R&D program of the Korea Ministry of Knowledge and Economy (MKE) and the Korea Evaluation Institute of Industrial Technology (KEIT). (2008-S-031-01, Hybrid u-Robot Service System Technology Development for Ubiquitous City)
3. Averaged Cell-Cost on Non-uniform Cost Maps An average is a statistical measure to represent central tendency of data. It is the expected value when we randomly
The 7th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 2010)
Algorithm 1 SearchPath(cstart , cgoal ) 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12: 13: 14: 15: 16: 17: 18: 19: 20: 21: 22: 23: 24: 25:
cstart .g := 0 cstart .parent := cstart open := ∅` ´ open.Push cstart , cstart .g + h(cstart , cgoal ) close := ∅ while open 6= ∅ do cbest := open.Pop() if cbest = cgoal then P = close.TraceParentsFrom(c best ) ˆ ˜ return success, P end if close.Push(cbest ) for each cchild adjacent to cbest do if cchild ∈ Mempty and cchild ∈ / close then [cost, parent] := CalculateCost(cchild , cbest ) if cchild ∈ open and cost < cchild .g then open.Remove(cchild ) end if cchild .cost := cost cchild .parent ` := parent ´ open.Push cchild , cost + h(cchild , cgoal ) end if end for end while ˆ ˜ return failure, ∅
(a) 4x4 example
(b) 5x3 example
Fig. 2. Two small grid maps present examples of line-of-sight between a pair of cells. Each cell-cost is presented at the bottom-right corner of cell.
Algorithm 2 CalculateCost(cchild , cbest ) 1: 2: 3: 4: 5: 6:
if IsNoCollision(cbest .parent, cchild ) = true then parent := cbest .parent else parent := cbest end if ˆ ˜ return parent.g + d(parent, cchild ), parent
pick up a value from the data.
Fig. 3. A histogram of the ratio of overall cost with respect to distance between two given cells (c.f. the number of bins: 10)
3.1 Simple Average
the result as 5.0, but the improved average (2) deduces 4.0. The improved one returns the true value this time, but it is not always correct such as Figure 2(b).
We can calculate a path-cost from c1 to c2 using average of cell-costs as follows: 1 X d0 (c1 , c2 ) = w ¯ d(c1 , c2 ) and w ¯= s(ci ) , (1) N
We performed Monte Carlo experiment to quantify accuracy of two methods. The accuracy is quantified as follows:
where L is a set of cells on the line-of-sight between c1 and c2 , N is its number of elements, and s(ci ) is cell-cost of ci . A function d0 (c1 , c2 ) replaces d(c1 , c2 ) in Algorithm 2. The arithmetic mean w ¯ compensates Euclidean distance d(c1 , c2 ) according to non-uniform cell-costs on the line-ofsight. Each cell-cost contributes to overall cost equally, but it is not always correct as shown in Figure 2(b). The lineof-sight passes through each cell with different amount of traverse. Especially, it goes through half of cell at both sides of line-of-sight.
d0 (c1 , c2 ) , (3) truth where truth is the true path-cost between c1 and c2 . Figure 3 presents accuracy by two methods. The improved average is slightly more accurate than the simple one when distance is short, and they are similar after 50 distance. As further works, the averaged cell-cost needs to reflect amount of traversal length of each cell. The average assumes that each cell-cost contributes to a path-cost evenly, but it is often incorrect as shown in Figure 2(b).
ci ∈L
3.2 Modified Average Considering Both Sides We can consider both sides of line-of-sight as follow: s(c1 ) s(c2 ) o 1 nX s(ci ) − − . (2) w ¯= N −1 2 2 ci ∈L
Such modified average can achieves better accuracy than the simple average. In case of Figure 2(a), the true path-cost from (0, 0) to (3, 3) is 4.0. The simple average (1) returns
4. Experimental Results and Conclusion
error =
References [1] D. Ferguson and A. Stentz, “The Field D* algorithm for improved path planning and replanning in uniform and non-uniform cost environments,” Carnegie Mellon University, Tech. Rep. CMU-TR-RI-05-19, 2005. [2] A. Nash, K. Daniel, S. Koenig, and A. Felner, “Theta*: Any-angle path planning on grids,” in Proceedings of AAAI Conference on Artificial Intelligence (AAAI), 2007. [3] S. Choi, J.-Y. Lee, and W. Yu, “Fast any-angle path planning on grid maps with non-collision pruning,” in Proceedings of IEEE Conference on Robotics and Biomimetics (ROBIO), 2010.
