3D Res (2016)7:14 DOI 10.1007/s13319-016-0092-9

3DR EXPRESS

Cornered Quadtrees/Octrees and Multiple Gateways Between Each Two Nodes; A Structure for Path Planning in 2D and 3D Environments Mohammad Hasan Namdari . Seyed Reza Hejazi . Maziar Palhang

Received: 3 February 2016 / Revised: 8 April 2016 / Accepted: 18 April 2016  3D Research Center, Kwangwoon University and Springer-Verlag Berlin Heidelberg 2016

Abstract In this paper, modified versions of quadtree/octree, as structures used in path planning, are proposed which we call them cornered quadtree/ octree. Also a new method of creating paths in quadtrees/octrees, once quadrants/octants to be passed are determined, is proposed both to improve traveled distance and path smoothness. In proposed modified versions of quadtree/octree, four corner cells of quadrants and eight corner voxels of octants are also considered as nodes of the graph to be searched for finding the shortest path. This causes better quadrant/ octant selection during graph search relative to simple quadtrees and octrees. On the other hand, after that all quadrants/octants are determined, multiple gateways are nominated between each two selected nodes and path is constructed by passing through the gateway which its selection leads in shorter and smoother path. Proposed structures in this paper alongside the utilized path construction approach, creates better paths in terms of path length than those created if simple trees are used, somehow equal to the quality of the achieved paths by framed trees, meanwhile interestingly, M. H. Namdari (&)  S. R. Hejazi Department of Industrial and Systems Engineering, Isfahan University of Technology, Isfahan, Iran e-mail: [email protected] M. Palhang Artificial Intelligence Laboratory, Department of Electrical and Computer Engineering, Isfahan University of Technology, Isfahan, Iran

consumed time and memory in our proposed method are closer to the used time and memory if simple trees are used. Keywords Quadtree  Octree  Path planning  Robot  2D/3D environments  Framed  Cornered

1 Introduction Unmanned vehicles are used increasingly in border control, weather forecast, firefighting missions and much more applications, and this shows the quick development and evolution of mobile robots and meanwhile organizations’ will to consider roles and missions which unmanned systems can accomplish [1]. The growing trend of using robots makes it necessary to utilize and control these tools efficiently. Actions to reach an autonomous vehicle are categorized in three groups; mapping and environment modeling, path planning and finally control system related activities [1]. The aim of path planning, as one of the most challenging issues of autonomous vehicles guidance, is to find the optimal path from the start point to the goal point in a reasonable time and in a manner in which obstacles are avoided [2]. Among discretized methods of representing environment, those that use uniform grid representation need large amount of memory, part of which is

123

14

Page 2 of 18

because of the cells that are not important and may never be passed [3]. This more than enough number of cells representing the environment also affects the time needed to find the shortest path. Quadtree was suggested in 1974 in Finkel and Bentley [4] for two dimensional environment applications and octree was presented in 1980 in Meagher [5] for three dimensional environment applications. These structures were used in different applications and one of those, was environment discretization. According to their more efficient manner of discretization relative to uniform grid representation, they are used both in 2D and 3D path planning. Although quadtree and octree structures may not be able to find true sequence of nodes which contain the achievable shortest path in some conditions such as when an obstacle is located on or near the boundary of a large free quadrant. Figure 1 shows one of these conditions in which path A is selected if simple quadtree is used while true achievable shortest path is path B if uniform grids were used. Framed quadtrees were suggested in 1995 in Chen et al. [8] for two dimensional environment. Their counterparts in three dimensional environment are framed octrees [8]. These structures were proposed to solve the above mentioned problem of simple quadtrees and octrees. Although they find better paths in terms of path length, but the time and memory needed to find the shortest path if these structures are used, make them undesirable especially in large examples. We in this paper propose new data structures both for 2D and 3D environments which remedy drawbacks

Fig. 1 Example of simple quadtrees malfunctioning in determining shortest path [6, 7]

123

3D Res (2016)7:14

of simple quadtrees/octrees in path planning application and meanwhile consume less time and memory than what is needed if framed quadtrees/octrees are used. The first problem of using simple quadtree/octree is that the graph search algorithm may not be capable of determining the true sequence of quadrants comprising the shortest path and this is because of existence of large quadrants/octants. The main problem confronting such large quadrants/octants originates from the fact that only one point may not be a good and enough representative of such big quadrants. In this paper, we add four/eight corner cells to each quadrant/ octant to increase the number of representatives of each node during path planning. This approach adds constant number of extra nodes in contrary to framed quadtrees/octrees in which number of added nodes is linearly/quadratic proportional to the length of each quadrant/octant. Adding corner cells can only help us to find true sequence of nodes containing the achievable shortest path. To generate an acceptable final path, we use different gateways between each two consecutive nodes in the sequence determined before. Number of gateways is equal to the length of smaller node of each two consecutive nodes. Between each two successive nodes, the path is passed through the gateway which results in shorter and smoother path. The remainder of this paper is organized as follows: section two reviews the related researches in the field of path planning, especially in discretized environments. Section three is about first part of our proposed method; cornered quadtrees/octrees, and part two of

3D Res (2016)7:14

our proposed method; multiple gateways between each two nodes and hints of path generation. This section also addresses some properties of our proposed method of path planning. Section four illustrates experimental tests and results and finally section five concludes this paper.

2 Related Researches One of the main necessary features of any path planning algorithm is its time efficiency which enables the algorithm to be applicable in changing environments. In other words, in many applications, path planning problems have to be solved in real time. Regardless of the manner of representing the environment, a search algorithm is needed and this search algorithm gets limited access to the CPU and memory resources. So it is not a choice, but a requirement for any path planning algorithm to be efficient in consuming any type of resource and meanwhile finding a good path in a reasonable time. There are two approaches of mobile robot path planning, each one use a different manner of representing the world; exact representation and discretized representation [3, 9]. In exact representations, complexity depends on the number of obstacle related features such as number of obstacles or obstacle facets. Three of more known exact representation methods are Visibility graph, Voronoi diagram and trapezoidal decomposition. The first two are roadmap methods. Visibility graph was proposed by Lozano-Pe´rez and Wesley [10]. Visibility graphs are based on the knowledge that, the shortest path grazes polygonal obstacles at their vertices. In this method a roadmap of lines connecting each vertex to all visible vertices from its point of view is created and used as a graph which is passed to graph search algorithms [11]. Voronoi diagram is another roadmap method which builds a skeleton that is maximally distant from obstacles, and finds the minimum distance path that follows this skeleton. [12] proposed using Voronoi diagram for practical unmanned helicopter operation. The last exact method mentioned in this section is trapezoidal decomposition. In this method, free space is divided into trapezoidal regions by dividing it with vertical lines from each of the obstacle vertices. The vertical lines do not bisect the obstacles themselves [11].

Page 3 of 18

14

In discretized approach of representing environment, complexity can be controlled by modifying the cell size. Although in some forms of discretized world, path planning for outdoor environments can be computationally expensive. Uniform grid representation is an example, in which in parts of environment without any obstacles, many cells are required to encode these empty regions, and all of them would be processed for finding a path through an empty area, many more than what is really needed. According to the created efficiency of map representation if quadtrees/octrees are utilized, among all of their applications, one is path planning [3]. A quadtree is based on successive division of each gray node (containing both free and full cells) into four children. This division occurs for all nodes which contain both free (white) and full (black) cells unless a stop criterion is reached. This structure allows more efficient representation of the environment than regular equal size grids due to representing large free spaces using less cells. Although, using this structure does not guaranty the optimality of the created paths [3]. As it is mentioned both in Yahja et al. [3, 13], generated suboptimal paths, if quadtrees are used, are because of path crossing through centers of each quadrant. Although the mentioned method in Yahja et al. [13] is not the only way of creating paths in quadtrees. It is noted in Ghosh et al. [14] that another approach of creating paths through quadrants is joining the mid points of the entry and exit boundaries of each quadrant. While Ghosh mentions that this approach is safer according to guarantied passing of the path inside free quadrants, it seems that it also creates shorter paths relative to the previous method which joins the centers of quadrants. Nevertheless, using any path generation approach, suboptimal created paths are yet possible if simple quadtrees are used and this is mainly because of setting quadrants centers as graph nodes which leads in incorrect sequence of quadrants determined by graph search algorithm. Octrees are equivalent structure of quadtrees in three dimensional environments. All steps of creating an octree is same as quadtrees except that each node is subdivided into eight children. In path planning, utilizing framed quadtrees/octrees partially solves the problems of using simple quadtrees/octrees. In framed trees, border cells of a quadrant/octant are also considered as passable nodes

123

14

Page 4 of 18

and this increases the number of permitted angles of direction, which results in shorter and smoother paths than what is created if simple quadtree/octree is used. These two structures use much more memory relative to simple quadtrees/octrees and are even more memory consuming relative to regular equal sized grids in uniformly cluttered environments [3]. As an example, in a 128 9 128 regular sized grid environment containing 16384 cells, obstacles of an example which creates 1249 quadrants if simple quadtree is used for environment representation, would be represented by 6678 cells if framed quadtree is utilized. Regardless of the features of above mentioned structures, both of quadtrees/octrees and framed quadtrees/framed octrees have been used in robot path planning problems in previous researches. Yahja et al. [13] investigates path planning in changing 2D environments using D* algorithm through framed-quadtree structure. They define how to calculate the neighborhood relationships of the framed cells of each quadrant initially and update them in case of perceived change in surrounding environment. They insist that, as the world becomes denser, number of border cells increases while number of links between border cells decreases. The work presented in Ghosh et al. [14] uses a fuzzy quadtree framework for path planning of a micro air vehicle (MAV). They use conventional quadtrees and generate paths through joining midpoints of entry and exit boundaries of each quadrant which is set as one of selected path nodes. They note that generating path in this manner guaranties the robot safety. Voros presented distance map implementation on simple and extended quadtrees and octrees in Voros [15]. He also noted that joining the middle of intersection of the entry and exit boundary edges of a quadrant would result in a safe paths. He wrongly stated that the optimized path through a free node is such line mentioned above considering previous and following nodes. Voros in this research used the famous example which shows simple quadtrees malfunctioning in finding the shortest path. The example which was illustrated in Fig. 1. This figure which was first used by Chen et al. [8] shows that in some examples in which an obstacle is located on or near the boundary of a large free quadrant, using simple quadtrees cannot yield to finding the shortest path. Voros proposed a so-called extended quadtree structure, mainly presented in

123

3D Res (2016)7:14

Voros [16] to solve this issue. Briefly speaking, a k-extended quadtree is defined as the quadtree, where the nodes of interest (black or white) correspond to the full grid of cells of required k-level dimensions. The main drawback of this new structure is that created paths resemblance to the optimal path depends on k-level and as k-level decrease (to reduce path length) more memory is needed. Guanglei and Heming [17] proposed a 3D path planner for autonomous underwater vehicles (AUV). They used octrees to model 3D environment and an ant colony algorithm for finding the shortest path. Xu et al. [18] is a real time 3D navigation method for autonomous MAVs which searches for an optimal trajectory in an octree-partitioned search space. Using octree results in fewer number of encoded states and therefore less memory consumption. They use state lattice which is a discrete representation of configuration space. It comprises a set of states, and transitions between them can be represented by a series of motion primitives. Modeling this lattice using octrees is the main contribution of this research. To remedy the suboptimality of the created paths as a result of using simple quadtrees/octrees, they use a somehow similar approach to so-called extended-quadtrees presented in Voros [15]. Zhang et al. [19, 20] proposed a reversed D* path planning for changing environments using a framedquadtree. Zhang along his co-researchers [19, 20] proposed framed-quadtree based path planning in 2D environments using hybrid simulated annealing and ant colony optimization algorithm. Colombo et al. [21] introduced a motion planning algorithm for crowded environments. They utilized simple quadtree structure. They used Dijkstra as a graph search algorithm and took mid-point of edge of smaller of any two neighboring quadrants as graph nodes. Abbadi and Prenosil [22] presented a static path planning algorithm based on simple quadtrees. They used three different methods of weighing quadrants to obtain different paths in terms of path safety. Hernandez et al. [23] presented a collision free path planner for AUVs. They uses octree-based representation of the environment along optimal rapidlyexploring random trees (RRT). They uses octomap presented in Hornung et al. [24] as an octree based representation. Omar et al. [25] presented a manipulator path planning problem in 2D environment and used a

3D Res (2016)7:14

quadtree structure for environment representation. They used artificial potential field as path finding algorithm. In terms of improving path generation through quadrants/octants, two other researches worth to be mentioned. First is [26] in which created paths in quadtrees are improved using visibility graphs, but the resulting paths are not optimal to the resolution of the smallest cell [13]. The second is [27] where balanced quadtrees are introduced, in which any two neighboring quadrants differ at most a factor of two in size and one in depth. The latter has somehow similar drawback of extended quadtrees mentioned above. According to above mentioned researches and despite better results of using framed-quadtrees/ octrees relative to simple trees in terms of optimality, it is apparent that most recent researches has been conducted using simple quadtrees/octrees. This seems mainly because of the point noted in Yahja et al. [3, 13], they require more memory than even regular grids in highly cluttered environments. And obviously when framed trees are used, much more memory and time is needed relative to simple quadtrees/octrees. So it is better to look at the drawbacks of simple quadtree/octree which resulted in framed quadtree/ octree. The main problem is that generated paths are suboptimal due to large quadrants/octants affecting the shortest path calculations. The main problem confronting such large quadrants/octants originates from the fact that only one point may not be a good and enough representative of such big quadrants/octants. But the approach of solving this problem in framedtrees does more than what is needed. Our proposed structure solves the problem in a thriftier way.

Page 5 of 18

14

structures is their efficiency in representing the world relative to equal sized regular grid approach. It was also noted that because of sub-optimal created paths if simple quadtree/octree is used to represent an environment, another data structure was proposed, named framed quadtree/octree. They create better paths both in terms of length and smoothness, but added nodes in this structure wastes much more memory than simple quadtree/octree and even relative to regular grids if the environment is highly cluttered. So it would be a fine achievement if a structure is proposed which creates paths almost similar to framed-trees created paths without such a memory consuming behavior. To do so it is better to investigate deeply the main reasons of simple quadtree/octree malfunctioning in path planning problems. For simpler visualization and understanding, examples of this part are presented based on quadtrees. We use famous example used in Chen et al. [8] and it is assumed that a graph search algorithm such as Dijkstra [28] or A* [29] is used to find the shortest path. In Fig. 2 if R is the current position of the robot and G is the goal point, using simple quadtree leads in selecting path 1 which is not the shortest path. This shows that the first problem of using simple quadtree is that the graph search algorithm may not be capable of determining the true sequence of quadrants comprising the shortest path. As it is shown in Fig. 2, this is because of existence of large quadrants. Using center point of each quadrant as graph nodes which is to be

3 Proposed Method In this section, first, our proposed structures, cornered quadtree/octree, are defined. Then the method of generating paths using multiple gateways is described. Finally some properties of proposed path planning method is evaluated. 3.1 Part One: Cornered Quadtree/Octree As mentioned in previous section, quadtrees/octrees are data structures used in vast range of applications, one of which is robot path planning in 2D and 3D environments. The main advantage of using these

Fig. 2 Effect of the representative point of a quadrant on path planning

123

14

Page 6 of 18

searched, causes the graph search algorithm to assume the long distance to the center of for example, quadrant A, the only way of passing through quadrant A and thus rating this quadrant not so appropriate to be selected. In some of previous researches such as [21], mid-point of the edge of the smaller of any two neighboring quadrants is taken as graph nodes, example the red square between quadrants A and B in Fig. 2. This may reduce the severity of path finding malfunctioning, but may lead in an improper path selection by the graph search algorithm yet. The main problem confronting such large quadrants originates from the fact that a point (whether the center point or the midpoint of a common edges) may not be good and enough representatives of such big quadrants. It seems that the philosophy of proposing framed-quadtrees is also to increase the number of representatives of each quadrant to guaranty achieving the correct sequence of nodes and better paths in terms of path length. But framed-quadtrees are the opposite side of simple quadtrees, while the latter use not enough number of representatives for especially big quadrants, the former use much more than what is needed as representatives of a quadrant. According to Fig. 2 if green square at the bottom right of quadrant A and a similar one at the bottom left of quadrant B are considered as representatives of their quadrants too, proper sequence of quadrants containing achievable shortest path can be distinguished by the graph search algorithm. It is apparent that all corner cells of a quadrant should be considered as representative of a quadrant as well as its center point. This helps determining true achievable shortest path pavement without much memory and time overhead. We in this paper introduce cornered quadtrees in which four corner cells of each quadrant bigger than specific size, which can be set by user, are also considered as nodes of graph search algorithm. Similar structure in 3D environment is called cornered octrees in which eight corner voxels of each octant bigger than specific size are also considered as nodes of graph search algorithm. Figure 3 illustrates an example, represented using both of simple quadtree and cornered quadtree. It can be seen in Fig. 3 part (b) that not all leaf nodes, but only those larger than specific size have corner cell. That specific size can be set by user and its decrease results in better quality output and meanwhile more memory and time consumption.

123

3D Res (2016)7:14

Using cornered quadtree/octree, all leaf nodes of the tree, plus corner cells/voxels are given to the graph search algorithm as the graph nodes. But after that the search algorithm finds the shortest path, maybe containing some of corner cells/voxels, the probable corner cells/voxels selected as graph nodes of the shortest path are discarded and their corresponding leaf node (which they are corner of) is taken into account. This way a more appropriate sequence of quadrants/octants are selected which are inputs of the path generation phase. Figure 4 shows an example in which sequence of selected quadrants connecting start node (in green) and goal node (in red), is compared in simple and cornered quadtree. As it can be seen in Fig. 4, using cornered quadtree results in more reasonable sequence of quadrants, capable of generating shorter and probably smoother paths, relative to using simple quadtree. One point that should be carefully treated, is how to determine the neighborhood of corner cells/voxels with other corner cells/voxels as well as other free leaf nodes. Neighborhood relations of leaf nodes of the quadtree or octree is calculated using the method proposed in [30]. As an obvious rule, all corner cells/ voxels of any free leaf node are neighbors of each other and their corresponding leaf node. They also are neighbors of leaf node neighbors of their corresponding leaf node. But they are not neighbors of necessarily all of corner cells/voxels of leaf node neighbors of their corresponding leaf node. Figure 5 depicts last mentioned situation, and for convenient understanding, it is an example on cornered quadtree. In this figure while green corner cell of quadrant B is neighbor of all corner cells of quadrant A, red corner cell of quadrant B obviously is not neighbor of southeast corner cell of quadrant A, because almost all of the link between these two corner cells is not laid inside quadrants A and B. If this exclusion is not considered in our calculations, graph search algorithm may select incorrect sequence of quadrants. However if a link is partially outside two neighboring quadrants, like the link between red corner cell and the northeast corner cell of quadrant A, it can be connived. To exclude ‘not really neighbor’ corner cells neighborhood, location of two points on each link, each third of the link length far from ending corner cells (blue stars in Fig. 5), are examined. If none of these points are located inside either first or second

3D Res (2016)7:14

Page 7 of 18

14

Fig. 3 A 2D example represented using a simple quadtree, b cornered quadtree

Fig. 4 A 2D example and selected sequence of quadrants by a graph search algorithm using a simple quadtree, b cornered quadtree

quadrant (yellow ones in Fig. 5, the corner cells at the end of the mentioned link are not neighbors. 3.2 Part Two: Multiple Gateways and Path Generation First problem of simple quadtrees/octrees as mentioned above can be solved using our proposed structure called cornered quadtree/octree. After determining the proper sequence of quadrants/octants using cornered quadtree/octree, an efficient path generation

algorithm is needed to create a possible smooth and short path through selected quadrants/octants. We again use the famous example used in Chen et al. [8] to explain our idea. According to Fig. 6, creating a path using midpoint of the common edges of quadrant A and B (as two nodes in the sequence of selected quadrants which contain achievable shortest path) would not be of interest neither in terms of path length nor smoothness. Inspired by the framed quadtree philosophy of increasing number of angles of direction, nominating multiple gateways between each two

123

14

Page 8 of 18

3D Res (2016)7:14

Fig. 5 Evaluaitng neighborhood of corner cells of two adjacent leaf nodes

successive quadrants/octants in the sequence of selected quadrants/octants, and selecting the best one in terms of path length and smoothness would result in totally better created paths relative to using midpoints. As shown in Fig. 6, path 2 is both shorter and smoother than path 1. In this paper we introduce a path generation method named multiple gateways. In this method in 2D environment, only for quadrants in the sequence of selected quadrants by the graph search algorithm, the common edge between each two successive quadrants would contain multiple available gateways that path can pass from. Number of gateways is equal to the edge length of smaller quadrant of the above mentioned two. The next question is how to select among gateways. In Fig. 6, assuming that the point between quadrants A and B is to be selected, previous point (PP), near future point (NFP) and far future point (FFP) are determined. Yellowed quadrants show selected sequence of quadrants containing shortest path, and midpoint of each common edge is marked using a red square. If the number of gateways is greater than one, we always select the gateway closer to both the line between PP and NFP and the line between PP and FFP. The influence of proximity to each of the above lines is equal and has similar weight. The gateway is selected using Eq. 1. g ¼ gi whichminimizesðw1  ðjgi  PPj þ jgi  NFPjÞ þ w2  ðjgi  PPj þ jgi  FFPjÞÞ ð1Þ

123

Fig. 6 Path generation using multiple gateways

where g is selected gateway and gi . is gateway i. of all nominated gateways. w1 and w2 are weights of proximity to near and far future line which we set them equal. NFP and FFP guide our gateway selection in the direction of following quadrants of the selected sequence. According to the fact that we do not know which gateway of the following quadrants would be selected, midpoints of the common edges are set as NFP and FFP. And FFP is added to remedy the probable negative effect of using midpoint NFP especially if following quadrants are large. All steps of gateway nomination and selection in octrees are similar except that the number of gateways is equal to the edge length of smaller octant powered by two and gateways are scattered over the common face of the octant. Figures 7 and 8 show two examples in 2D environment and 3D environment using simple quadtree/octree, framed quadtree/octree and cornered quadtree/octree. Path generation in our proposed cornered trees is done using above mentioned method; multiple gateways. In both 2D and 3D environment examples, the start node is green and the goal node is red. Also in 3D environment test, framed octree path is not visible due to the fact that all the outside area is covered by framed nodes. From Fig. 7 it is clear that created path using framed quadtree and cornered quadtree in this example, are much shorter and smoother than path of simple

3D Res (2016)7:14

Page 9 of 18

14

Fig. 7 A 2D example and created path by a graph search algorithm using a simple quadtree, b framed quadtree, c cornered quadtree

quadtree. On the other hand despite little better quality of the path created using framed quadtree relative to our proposed method, framed quadtree memory usage and needed time to compute shortest path is far more than what our proposed structure uses and needs. 3.3 Properties of Our Proposed Method In this subsection, some properties of our proposed method, considering both cornered trees and path generation using multiple gateways, is discussed. Our proposed method is resolution complete. In other words, it finds a path from the start point to the goal point, if one

exists. The term ‘resolution’ is related to the discretization of the solution space. Also our proposed method is correct, which means it only finds unblocked and free paths. In terms of optimality, our proposed method is not optimal, but it can be proven that created paths using our proposed structure are shorter than or equal to those created using simple quadtrees/octrees, if similar path generation methods are utilized. 3.3.1 Completeness Theorem 1 Provided all passable regions are modeled and not ignored according to the resolution

123

14

Page 10 of 18

3D Res (2016)7:14

Fig. 8 A 3D example and created path by a graph search algorithm using a simple octree, b framed octree, c cornered octree

of the quadtree/octree, utilizing cornered quadtree/ octree to discretize the environment and using a graph search algorithm such as A*, results in finding a path from the start point to the goal point if one exists. Proof if both the start point and the goal point are located in free space of the environment, and as mentioned before, all passable regions (including start and goal points) are modeled according to the resolution of the quadtree/octree, then the start and the goal point would be represented by two of quadtree/octree free leaf quadrants/octants. Considering free leaf quadrants/octants as nodes of a graph, they are passed to a graph search algorithm such as A*. A* expands one node in the open list during each iteration and removes it from the open list. According to the fact that number of nodes is finite, the open list finally becomes empty and A* terminates, if it has not reached the node representing the goal point yet. If A* terminates because the open list has become empty, then there is no path between the nodes representing

123

start and end points and if it terminates because that the node of goal point is expanded, there would be a path from the node representing the start point to the node of the goal point, which van be extracted by following the parents of the node of goal point backward. h 3.3.2 Correctness Theorem 2 If a path exists between the start and the goal point according to the resolution of the quadtree/ octree, utilizing our proposed multiple gateways method for path generation, generated path is always unblocked. Proof if a path exists between the start and the goal point and the sequence of nodes between these two points is determined and our proposed multiple gateway path generation method is used, then except the start point and the end point, all other vertices of the generated path are placed on common edges/faces

3D Res (2016)7:14

of any two successive quadrants/octants of the aforementioned sequence of nodes. The first segment of the path is a line between start point (which is inside the first quadrant/octant of the sequence) and a point on the common edge/face of the first and the second quadrants/octants of the sequence. So the first segment is totally inside the first node. Similarly the last segment is totally inside the last node of the sequence. All other medial segments are lines connecting two edges/faces of a free leaf quadrant/ octant and consequently are totally inside that quadrant/octant. h 3.3.3 Optimality Theorem 3 Created paths using the sequence of quadrants/octants, if a graph search algorithm is used based on cornered quadtree/octree structure, are shorter than or equal to those created if the graph search algorithm is used based on simple quadtrees/ octrees, provided that similar path generation methods are utilized. Proof if cornered quadtree/octree structures are used to model an environment in path planning problems, four/eight corner cells are added to each quadrant/ octant and consequently five/nine points (including the center point of each quadrant/octant) are passed to the graph search algorithm corresponding to each quadrant/octant. Considering the fact that in simple quadtree/octree, only center points are passed to the graph search algorithm, the added corner cells to quadrants/octants (and also to the nodes of the graph to be searched) can be considered as a relaxed constraint in the search space. In any optimization problem, relaxing a constraint would lead in better or equal result relative to constrained problem. h

4 Experimental Tests To evaluate our proposed method, we have designed series of random generated examples both in 2D environment and 3D environment and used simple quadtree/octree, framed quadtree/octree and proposed cornered quadtree/octree as representation structures. A* graph search algorithm was used to find shortest paths in each type of representation but other graph search algorithms, especially those used for changing

Page 11 of 18

14

environments such as D* [31] or D* Lite [32] can be used too. Path generation in our proposed structure, cornered quadtree/octree, is done using multiple gateways. Also in 2D environment, two uniform gird based search algorithms, A* and Theta* [33] are also included in our tests for better comparison. Theta* is included as an any angle search algorithm which is probably our benchmark model in terms of created paths quality. All methods were tested using the following hardware and software: • • • •

CPU: Intel CoreTM i5 CPU M480 @ 2.67 GHz 2.67 GHz. Internal memory (RAM): 4 GB. OS: Windows 7 Home Premium 64-bit 2009. Programming language: Microsoft visual C# 2010.

In two dimensional environment, quadtrees of depth 4–11 (i.e. 16 9 16–2048 9 2048) and their equivalent uniform grids are tested. In three dimensional environment octrees of depth 3–7 (i.e. 8 9 8 9 8–128 9 128 9 128) are the basis of our evaluation. In each depth, 30 randomly generated examples are assessed among which some may not contain a path because of probable start or goal node entrapment by obstacles. Position of the start node and the goal node and number, size and location of obstacles is determined randomly. Main indicators to rank different structures performance are created path length and smoothness which both represent the path quality. The former is clear but the latter is calculated using the average deviation of the created path. This is calculated using sum of angles between created path segments. By ‘angle between created path segments’ we mean h in Fig. 9. As h increases the path contains more deviation and is less desirable in terms of smoothness. Apart from path quality indicators, time and memory performance of different structures are also evaluated. These two indicators help us to analyze how efficient is each structure regardless of its created path quality. In other words, it is possible that one structure creates high quality paths, but meanwhile its time and memory consumption is not acceptable. In this paper, A* is used as graph search algorithm in different tree structures and time performance of each above mentioned structures is assessed by calculating the average time consumed by the graph search algorithm to find the shortest path. In 2D environment

123

14

Page 12 of 18

3D Res (2016)7:14

4.1 Created Path Quality According to the fact that absolute values of created paths length using each structure are much greater than their differences, we consider the length of the path created using Theta* on uniform grids as a benchmark. Figure 10 illustrates difference of the average length of paths created using simple, framed and cornered quadtree and also A* on uniform grids with the average length of paths created using Theta* on uniform grids as a benchmark in 2D examples. Values of this differences (in percentage) can be tracked by line chart scaled on the left axis of the chart. Also absolute values of the length of created paths using Theta* on uniform grids are represented using columns and their value is based on the right axis. In 3D environment, among three tested tree structures, framed octree was selected as our benchmark in evaluating path length results. Figure 11 illustrates difference of the average length of paths created using simple and cornered octree with the average length of paths created using framed octree as a benchmark in 3D examples. Values of this differences (in percentage) can be tracked by line chart, scaled on the left axis of the chart. Also absolute values of the length of

Fig. 9 The angle between created path segments

A* and Theta* search algorithms are also implemented on uniform grids and the average time consumed by each one to find the shortest path would be their time indicator on uniform grids. While evaluating our proposed method, the time consumed by path generation algorithm is also taken into account. Queue length of the open list of the graph search algorithm is also assessed which in fact, reflects the origins of time consumption during graph search in each tree type. Finally memory usage is compared based on total number of graph nodes as a result of using each kind of above mentioned quadtrees/octrees structures.

12.00

1800 1600

10.00 1400

Percent

1000 6.00 800

Path length

1200

8.00

600

4.00

400 2.00 200 0.00

4

5

6

7

8

9

10

11

Uniform Grid-Theta*

736.21

743.94

719.27

742.13

698.73

857.84

868.49

1675.22

Simple Quadtree

10.02

10.29

8.50

8.46

7.41

7.14

7.19

8.43

Framed Quadtree

6.79

3.70

3.08

1.91

1.41

0.98

0.52

0.19

Cornered Quadtree

7.58

4.66

4.10

3.99

3.54

3.85

3.20

3.07

Uniform Grid-A*

3.00

4.35

5.00

4.80

4.79

5.19

4.84

4.74

0

Tree depth Uniform Grid-Theta*

Simple Quadtree

Cornered Quadtree

Uniform Grid-A*

Framed Quadtree

Fig. 10 Length of paths created using Theta* on uniform grids (columns) and its difference (lines) with paths created using simple, framed and cornered trees as well as A* on uniform grids in 2D environment

123

3D Res (2016)7:14

Page 13 of 18

30.00

160

25.00

140

20.00

120

Percent

15.00

100

10.00

80

5.00

60

0.00 -5.00

40

-10.00

20

-15.00

3

4

5

6

7

Framed Octree

8.53

14.98

33.75

77.54

144.99

Simple Octree

9.27

14.36

13.06

13.15

23.76

-10.94

10.57

0.81

6.96

2.88

Cornered Octree

Path length

our examples, specific obstacle conditions which creates situations such as what is depicted in Fig. 1 may not occur in our examples, but yet cornered quadtree/octree usage results in better paths than simple quadtree/octree in terms of path length. Figures 12 and 13 assess second path quality related indicator; smoothness. As mentioned before we use sum of angles between path segments as a measure of path smoothness. As it can be seen in Table 1, in 2D environment, Theta* on uniform grids performs the best, and created paths by implementing A* on uniform grids are almost far out of the average

created paths using framed octree are represented using columns and their value is based on the right axis. Both of Figs. 10 and 11 show that both in 2D and 3D environment, cornered quadtree/octree paths are better in terms of path length than created paths using simple quadtree/octree. Also length of our proposed method created paths are 4 percent longer than the paths created by Theta* on uniform grids in average in 2D environment, and are only 2 percent longer than the paths created by framed octrees in average in 3D environment. According to randomly generation of

Fig. 11 Length of paths created using framed trees (columns) and its difference (lines) with paths created using simple and cornered trees in 3D environment

14

0

Tree depth

Framed Octree

Fig. 12 Sum of angles between path segments created using simple, framed and cornered trees as well as Theta* on uniform grids in 2D environment

Simple Octree

Cornered Octree

2000 1800 1600

Degrees

1400 1200 1000 800 600 400 200 0 4

5

6

7

8

9

10

11

Tree depth Simple Quadtree

Framed Quadtree

Cornered Quadtree

Uniform Grid-Theta*

123

Page 14 of 18

Fig. 13 Sum of angles between path segments created using simple, framed and cornered trees in 3D environment

3D Res (2016)7:14 3000 2500 2000 Degrees

14

1500 1000 500 0

3

4

5

6

7

Simple Octree

217.84

305.86

573.14

1123.59

2387.62

Framed Octree

206.72

279.81

444.29

834.70

1652.19

Cornered Octree

133.19

245.83

346.28

494.04

933.47

Tree depth Simple Octree

Framed Octree

Cornered Octree

Table 1 Sum of angles between path segments created using simple, framed and cornered trees as well as A* and Theta* on uniform grids in 2D environment Simple quadtree

Framed quadtree

Cornered quadtree

Uniform grid-A*

Uniform grid-Theta*

4

195.84

178.26

189.38

178.39

78.92

5

272.91

235.56

208.36

338.28

68.80

6

388.02

347.91

266.82

502.76

90.10

7

553.03

540.79

353.85

756.96

112.26

8

863.30

918.61

573.50

1641.72

182.50

9

1143.77

1305.91

738.82

3673.50

153.60

10

1338.29

1419.15

732.14

7941.00

135.71

11

1727.65

1712.70

806.74

17,872.44

142.63

810.35

832.36

483.70

4113.13

120.57

Average

range of smoothness if other methods are used. So in Fig. 12, we have excluded A* on uniform grids and compared smoothness of created paths of other four methods. In Fig. 12, it is apparent that, paths created using our proposed cornered quadtree are smoother than both of simple and framed quadtrees. In 3D environment tests, as mentioned before, uniform grid based methods are not included. Sum of angles between segments of paths created using each structure can be viewed in Fig. 13 in trees of depth 3 to depth 7. This figure also shows that in 3D environment, using cornered tree structure leads in smoother paths than if simple or framed trees are used.

123

4.2 Time and Memory Performance In this part, time and memory performance of each method is evaluated. First, average time consumed by A* algorithm to find shortest paths using simple, framed and cornered quadtree structures and also A* and Theta* on uniform grids is examined in 2D environment. Figure 14 depicts these information. Times are in milliseconds. This figure shows that methods on uniform grids alongside framed quadtrees cannot be good choices at all if time efficiency is a critical issue, and also we know that time efficiency is one of the most important prerequisites in real time systems.

3D Res (2016)7:14

14

250,000 200,000 Milliseconds

Fig. 14 Average time needed by graph search algorithm to find shortest path using simple, framed and cornered quadtrees and A* and Theta* on uniform grids in 2D environment

Page 15 of 18

150,000 100,000 50,000 0

4

5

6

7

8

9

10

11

Simple Quadtree

0.32

0.34

0.53

1.17

3.62

11.18

72.11

206.56

Framed Quadtree

0.53

0.95

7.30

47.34

296.73

2853.70

Cornered Quadtree

2.44

1.27

1.70

3.09

8.69

18.31

Uniform Grid-A*

0.48

0.90

6.15

45.20

339.98

2211.88 29940.94 216472.99

Uniform Grid-Theta*

2.35

3.13

10.43

47.32

333.80

2102.90 15130.60 127097.07

6393.57 27620.23 118.90

363.90

Tree depth

Framed Quadtree

Uniform Grid-A*

Uniform Grid-Theta*

Cornered Quadtree

300,000 250,000 Milliseconds

Fig. 15 Average time needed by graph search algorithm to find shortest path using simple, framed and cornered octrees in 3D environment

Simple Quadtree

200,000 150,000 100,000 50,000 0

3

4

5

6

7

Simple Octree

0.19

0.74

9.08

119.45

855.18

Framed Octree

0.34

8.51

413.56

11019.43

245252.08

Cornered Octree

0.44

2.35

24.45

292.13

1424.81

Tree Depth Simple Octree

In 3D environment, tests have been done using only tree based structures and Fig. 15 depicts the time consumed by the graph search algorithm using each tree structure. Both 2D and 3D environment tests show that in terms of time consumption, our proposed cornered tree structures performs a lot better than other structures which create high quality paths. The last indicator to be examined is memory usage. To do so, total number of created nodes using each tree structure and uniform grid is compared in 2D environment. The nodes in simple quadtree are free leaf nodes of the tree, in framed quadtree are framed

Framed Octree

Cornered Octree

nodes of free leaf nodes plus free leaf nodes of size one and finally in cornered quadtree are free leaf nodes plus four cornered cells of free leaf quadrants. In uniform grids, free cells are the nodes. Figure 16 shows these values in our examples. In 3D environment, only tree based structures are assessed, and the average of number of total nodes in case of using each structure is depicted in Fig. 17. From Figs. 16 and 17 it can be figured out that while cornered and simple quadtree/octree structures create somehow equal number of nodes which are to be searched by graph search algorithm, the number of

123

Page 16 of 18

Fig. 16 Average number of total nodes used in the graph search algorithm using simple, framed and cornered quadtrees and A* and Theta* on uniform grids in 2D environment

3D Res (2016)7:14 4,500,000 4,000,000 Number of graph nodes

14

3,500,000 3,000,000 2,500,000 2,000,000 1,500,000 1,000,000 500,000 0

4

5

6

7

8

9

10

11

Simple Quadtree

54.04

130.41

443.59

962.68

3069.03

7392.63

16252.97

34721.80

Framed Quadtree

161.79

553.66

2102.90

5992.00

18739.17

58376.27 165768.50 493990.13

Cornered Quadtree

74.46

216.21

767.17

1273.39

3315.93

7647.97

Uniform Grid-A*

190.21

810.48

3356.69

12914.57

43154.21 190973.73 860097.97 3922046.73

Uniform Grid-Theta*

190.21

810.48

3356.69

12914.57

43154.21 190973.73 860097.97 3922046.73

17558.30

38915.93

Tree depth Framed Quadtree

Cornered Quadtree

Uniform Grid-A*

Uniform Grid-Theta*

800,000 Number of graph nodes

Fig. 17 Average number of total nodes used in the graph search algorithm using simple, framed and cornered octrees in 3D environment

Simple Quadtree

700,000 600,000 500,000 400,000 300,000 200,000 100,000 0

3

4

5

6

7

Simple Octree

150.8571429

677.6181818

3731.4

23992.902

129801.5998

Framed Octree

334.7295918

2691.2

17900.8165

134283.8054

732077.3279

Cornered Octree

155.5102041

805.2630303

4887

30836.97

159735.5046

Tree depth Simple Octree

these nodes if framed quadtree/octree used is extremely greater than those two, and this difference is increased more and more as the tree becomes deeper. Number of total nodes used in graph search algorithms is far more than tree based structures.

5 Conclusion It is known that unmanned vehicles are increasingly used in different applications. Among actions to reach an autonomous vehicle, path planning is one of the

123

Framed Octree

Cornered Octree

most important ones. Among discretized approaches of representing surrounding environment of a robot, those that use uniform grid representation need large amount of memory. Quadtrees and octrees were introduced to solve this problem but they are unable to find true sequence of quadrants/octants in specific conditions and created paths are usually suboptimal. Framed quadtrees/octrees were then introduced to partly remedy the optimality problem of their previous versions, but they suffer from memory-related issues yet and need much more time than simple quadtrees/ octrees to find a path.

3D Res (2016)7:14

In this paper, a modified variation of quadtrees and octrees is proposed; cornered quadtrees/octrees, in which four corner cells of quadrants and eight corner voxels of octants are also considered as nodes of the graph to be searched for the shortest path. Better quadrant/octant selection during graph search, relative to simple quadtrees and octrees, is the result of using these additional nodes. Another tool to improve path length and smoothness is also proposed; a new method of creating paths in quadtrees/octrees after that quadrants/octants to be passed are determined, which we name it multiple gateways. Passing through the gateway which its selection leads in shorter and smoother path results in better quality paths. Experimental tests were conducted both in 2D and 3D environments. In 2D environment, two uniform grid based methods, A* and Theta*, were also tested to achieve more comprehensive comparison. Experimental tests show that proposed structures in this paper alongside utilized path construction approach, creates paths which their quality is somehow similar to the achieved paths using framed trees, but time and memory consumption are closer to the results of using simple quadtrees and octrees.

References 1. Huang, H. M., Pavek, K., Novak, B., Albus, J., & Messin, E. (2005). A framework for autonomy levels for unmanned systems (ALFUS). In Proceedings of AUVSI Unmanned Systems 2005. 2. Pehlivanoglu, Y. V. (2012). A new vibrational genetic algorithm enhanced with a Voronoi diagram for path planning of autonomous UAV. Aerospace Science and Technology, 16(1), 47–55. 3. Yahja, A., Stentz, A., Singh, S., & Brumitt, B. L. (1998). Framed-quadtree path planning for mobile robots operating in sparse environments. In Proceedings of IEEE International Conference on Robotics and Automation, 1998. (Vol. 1, pp. 650–655). IEEE. 4. Finkel, R. A., & Bentley, J. L. (1974). Quad trees a data structure for retrieval on composite keys. Acta Informatica, 4(1), 1–9. 5. Meagher, D. J. (1980). Octree encoding: A new technique for the representation, manipulation and display of arbitrary 3-d objects by computer. Electrical and Systems Engineering Department Rensseiaer Polytechnic Institute Image Processing Laboratory. 6. Chen, D. Z., Szczerba, R. J., & Uhran, J. J. (1995). Using framed-octrees to find conditional shortest paths in an unknown 3-d environment. Informe Te´cnico, 95–9.

Page 17 of 18

14

7. Chen, D. Z., Szczerba, R. J., & Uhran J. J. Jr. (1995). Using framed-quadtrees to find conditional shortest paths in an unknown 2-D environment (Vol. 95, No. 2). Technical Report. 8. Chen, D. Z., Szczerba, R. J., & Uhran J. J. Jr. (1995). Planning conditional shortest paths through an unknown environment: A framed-quadtree approach. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems 95. ‘Human Robot Interaction and Cooperative Robots, 1995 (Vol. 3, pp. 33–38). IEEE. 9. Latombe, J. C. (2012). Robot motion planning (Vol. 124). Berlin: Springer Science & Business Media. 10. Lozano-Pe´rez, T., & Wesley, M. A. (1979). An algorithm for planning collision-free paths among polyhedral obstacles. Communications of the ACM, 22(10), 560–570. 11. Goerzen, C., Kong, Z., & Mettler, B. (2010). A survey of motion planning algorithms from the perspective of autonomous UAV guidance. Journal of Intelligent and Robotic Systems, 57(1–4), 65–100. 12. Howlet, J. K., Schulein, G., & Mansur, M. H. (2004). A practical approach to obstacle field route planning for unmanned rotorcraft. 13. Yahja, A., Singh, S., & Stentz, A. (2000). An efficient online path planner for outdoor mobile robots. Robotics and Autonomous systems, 32(2), 129–143. 14. Ghosh, S., Halder, A., & Sinha, M. (2011). Micro air vehicle path planning in fuzzy quadtree framework. Applied Soft Computing, 11(8), 4859–4865. 15. Vo¨ro¨s, J. (2001). Low-cost implementation of distance maps for path planning using matrix quadtrees and octrees. Robotics and Computer-Integrated Manufacturing, 17(6), 447–459. 16. Vo¨ro¨s, J. (1998). Using extended quadtrees in robot path planning. In Proceedings of the Seventh Workshop on Robotics in Alpe-Adria-Danube Region, Smolenice (pp. 451–456). 17. Guanglei, Z., & Heming, J. (2013). 3D path planning of AUV based on improved ant colony optimization. In 32nd Chinese Control Conference (CCC), 2013 (pp. 5017–5022). IEEE. 18. Xu, S., Honegger, D., Pollefeys, M., & Heng, L. (2015). Real-time 3D navigation for autonomous vision-guided MAVs. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015 (pp. 53–59). IEEE. 19. Zhang, Q., Ma, J., & Xie, W. (2012). A framed-quadtree based on reversed d* path planning approach for intelligent mobile robot. Journal of Computers, 7(2), 464–469. 20. Zhang, Q., Ma, J., & Liu, Q. (2012). Path planning based quadtree representation for mobile robot using hybrid-simulated annealing and ant colony optimization algorithm. In 10th World Congress on Intelligent Control and Automation (WCICA), 2012 (pp. 2537–2542). IEEE. 21. Colombo, A., Fontanelli, D., Legay, A., Palopoli, L., & Sedwards, S. (2015). Efficient customisable dynamic motion planning for assistive robots in complex human environments. Journal of Ambient Intelligence and Smart Environments, 7, 617–633. 22. Abbadi, A., & Prˇenosil, V. (2015). Safe path planning using cell decomposition approximation. Distance Learning, Simulation and Communication, 2015, 8.

123

14

Page 18 of 18

23. Hernandez, J. D., Vidal, E., Vallicrosa, G., Galceran, E., & Carreras, M. (2015). Online path planning for autonomous underwater vehicles in unknown environments. In IEEE International Conference on Robotics and Automation (ICRA), 2015 (pp. 1152–1157). IEEE. 24. Hornung, A., Wurm, K. M., Bennewitz, M., Stachniss, C., & Burgard, W. (2013). OctoMap: An efficient probabilistic 3D mapping framework based on octrees. Autonomous Robots, 34(3), 189–206. 25. Omar, F. S., Islam, M. N., & Haron, H. (2015). Shortest path planning for single manipulator in 2D environment of deformable objects. Jurnal Teknologi, 75(2), 33–37. 26. Zelinsky, A. (1992). A mobile robot exploration algorithm. IEEE Transactions on Robotics and Automation, 8(6), 707–717. 27. De Berg, M., Van Kreveld, M., Overmars, M., & Schwarzkopf, O. C. (2000). Computational geometry (pp. 1–17). Berlin: Springer. 28. Dijkstra, E. W. (1959). A note on two problems in connexion with graphs. Numerische Mathematik, 1(1), 269–271.

123

3D Res (2016)7:14 29. Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics, 4(2), 100–107. 30. Namdari, M. H., Hejazi, S. R., & Palhang, M. (2015). MCPN, Octree Neighbor Finding During Tree Model Construction Using Parental Neighboring Rule. 3D. Research, 6(3), 1–15. 31. Stentz, A. (1994). Optimal and efficient path planning for partially-known environments. In IEEE International Conference on Robotics and Automation, 1994. Proceedings (pp. 3310–3317). IEEE. 32. Koenig, S., & Likhachev, M. (2005). Fast replanning for navigation in unknown terrain. IEEE Transactions on Robotics, 21(3), 354–363. 33. Daniel, K., Nash, A., Koenig, S., & Felner, A. (2010). Theta*: Any-angle path planning on grids. Journal of Artificial Intelligence Research, 39, 533–579.