SPATIAL GUIDANCE TO RRT PLANNER USING CELL-DECOMPOSITION ALGORITHM

SPATIAL GUIDANCE TO RRT PLANNER USING CELL-DECOMPOSITION ALGORITHM Ahmad Abbadi, Radomil Matousek, Pavel Osmera, Lukas Knispel Brno University of Technology Institute of Automation and Computer Science Technick 2896/2, 616 69 Brno Czech Republic abbadi83@gmail.com, matousek@fme.vutbr.cz, osmera@fme.vutbr.cz Abstract: Motion planning is problematic issue in robotics domain. In this paper we compare very well-known planners, the probabilistic path planning method Rapidly Exploring Random Tree (RRT) and spatial planner as exact Cells-decomposition (CD). We try to make some tradeoff between the efficiency of planning using CD in 2D space and dynamic of planning using RRT by giving the path-point of CD to RRTs planner as spatial guidance to overcome the drawback of this two planners. Keywords: RRTs, Cell-decomposition, hybrid planner, randomize samples-based planners, Robot, path planning, motion planning. 1 Introduction The motion Planning is essential part of robots models. It is a problem of finding a path for robot to move from one position to another, avoiding obstacles on the way and obey kinematic or dynamic constraints. Last decade many researches have been done for path planning problems. The complexity of the motion problem for robots differs from one robot model to others. It could be easy like holonomic in two dimensions without any constraints, or it could be complex like a robot which has complex kinematic and dynamic constraints. For example, from planning point of view, the flying objects movement in x, y, z axes, roll, pitch, yaw, the dynamics like speed, acceleration or other parameters can be considered as dimensions when we plan the motion from one position to another, the problem will be more complicated when more than one robot joint together to perform shared tasks e.g.[8]. Many algorithms were used in this area, e.g. potential field (Khatib,1986), neural networks, A*, genetic algorithm, bugs, cells decompositions, victor field histogram(vfh), Probabilistic Road Map(PRM), and Rapidly exploring Random trees (RRTs) and many others. Figure 1: a: principle of RRT, b: RRT in 3D, c: cell decomposition in offices-like schema In this paper we will present two types of algorithms for path planning: RRT as an example of random samples-based algorithms, and Cell Decomposition algorithm. In cell-decomposition algorithm we assume that the obstacle locations, shape and orientations are known. We explicitly prepare the space to plan a path. First, we find free spaces between obstacles. Then we execute the planning quires over these spaces. In comparison 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014 273

RRT does not make pre-assumption or pre-calculation of obstacle-free space. It tries to connect random points to a tree checking each of them for collisions. In the next sections we will shortly explain some details and weak sides of each algorithm. We propose an idea to use hybrid planner which exploit available spatial information to overcome the weakness of randomized algorithm and enhance the performance. The paper is divided into four sections. In the first and second sections we briefly talk about RRT algorithm and Cells Decomposition principles. The next section is about motion planning problem in high dimensions. It also contains the proposed solutions. The last section describes the experiments and the results of our tests. We conclude the paper by the summary of our results and plans of future research. 2 RRT (Rapidly Exploring Random Tree) RRTs algorithm is probabilistic algorithm used for exploring the spaces rapidly and planning paths. Originally RRTs is a single query planner. RRTs works efficiently for problems with high dimensions, nonlinear dynamics or differential constraints. It is also good in discrete or continuous spaces. In RRTs we do not need to make any pre-computing for the configuration space. The tree is grown by connecting random samples in free space. A new edge will not be added to the tree if there are any collision with obstacles. In high dimension we consider that the obstacle is not a rigid object, but it could be constraint e.g. it can have dynamic or kinematic constraints. The RRTs is also proofed to be probabilistically complete ([6],[3]). There are several main drawbacks of RRTs. It is not optimal, because it depends on probability and a random choice of next configuration. A tree contains a great number of redundant nodes. It has some difficulty to explore small areas and find the way throw narrow passages, because the probability to choose a configuration in these areas is small. Moreover, the probability to connect the chosen configurations with a tree without collision is also small. The generated paths are usually tortuous and have sharp curve. The RRT algorithm is very simple(see Fig. 1(a)). Let X R d is the configurations space and d N is dimension of the configurations space. X obstacle is the space occupied by obstacles. X free is the space free from obstacles. The start point is X init and the goals is X target. p X free is a path generated by the algorithm in X free. The algorithm of RRT is following. 1: Make X init as a root for a tree. 2: Pick a random point X rnd in the space. 3. Find the nearest neighbor of X rnd, named X near in a tree. 4: Try to grow a branch [X near, X new ] of length ɛ, where [X near, X new ] is a segment on the stright line between the nearest point X near and X rnd. 5: If no collision is detected, add X new as a new point to a tree. 6: Check if we reach X target. 7: Repeat step 2 to 6 until we reach the X target and return the path [2]. To improve the RRTs efficiency we mention here some previous works which surveyed and reviewed in ([1], [2]). To increase the probability of connection between initial and goal positions, some variants of RRT grow a second tree from the goal point or grow multiple trees trying to connect them [4]. Other RRTs variances try to extend the branch of tree as long as possible in order to cover the space fast and increase the possibility of trees connections. Variable Length RRT [9] proposes to change the way to extend a tree branch. It makes branches on open area longer than branches on semi-free areas. Thus, it decrease time to cover obstacle-free areas and increases the probability to build a tree through regions with many obstacles. Other variants of RRTs try to improve the quality of randomly chosen configurations. For example to extend the tree we can bias it to the goal point instead of a random point with some probability [8]. In another case we can choose point from the hull around the goal with some probability. Thus, a tree will grow towards goal area. It increases the possibility to find a path to the goal faster. In [5] the authors give a bias to a tree towards old path-points. In [11] it was proposed heuristically-guided RRT which chooses the next point based on Voronoi regions, and the quality of a path to this point. Another approach is to choose a random point along with k-nearest points from the tree and then choose the best point from them. Figure 2: Cell Decomposition principle a:first step, b: next step 274 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014

3 Cells decomposition Cell decomposition is one of the first applicable solution for path planning. The algorithm (Fig. 3) aims to find free areas (cells not occupied by obstacles) in the configuration space, and build a graph of adjacency for these cells. Finding free cells is not easy task especially in high dimensions. Many approximation solutions were proposed to increase the efficiency of this algorithm. One of these solutions in (2d) is to use a sweep line on x-axis (Fig. 2), Then find the intersections with obstacles-edges, in order to generate the cells. To model the process let us assume X is the configuration space. X free is the obstacle-free space. X obst is obstacle-occupied space. V = {v 11, v 21,..., v ij,... } R d ; v i < v i+1, i, j N + ; d is dimension of configuration space; i represent the vertex index in obstacle j. V is ordered set of all vertices of obstacles in addition to outer contour of configuration space. E = {e 11, e 21... e ij,... } V d ; i is index of the side of obstacle j. E is set of all edges which create the obstacles and space contour. V visited is a set of all vertices and intersection points which we visited before. P intersect is set of upper and lower intersection points to tested vertex v. CD is generated cells. V neighbor is set of vertices witch we visited it before and fall in same edge e with P intersect or with v, CELLS is set of all cells in X free. The algorithm output is horizontal free areas (see Fig. 2-a). but for better use as spatial planner we add another step to divide the cells horizontally Fig. 2-b. Base on generated cells we build graph of adjacency; nodes is the cells ID and edge is the connection between the cells. By this way the problem of navigation and path planning turned into graph search problem. For example, when we want to plan a path between two positions, we find cells which contain the positions, then we search over the graph for path. Transform motion planning in Algorithm : CellDecomposition 1. V spaceedges V 2. V order(v ) 3. F oreach(v V ){ 4. P intersect intersection(v, E) v 5.V neighbor onsameedge(v visited, P intersect) 6. CD constructcell(v neighbor, P intersect ) 7. CD CD / X obst 8. V visited V visited \V neighbor 9. V visited V visited P intersect ; P intersectisusedinobstacle freecd} Figure 3: Cell Decomposition algorithm spatial environment to very well-known and studied problem like graph give us many advantages.optimal path can be calculated by using A*, Dijkstra or other good search algorithm. We can use the spatial knowledge about the cells to produce weights of edges, i.e. if we need short path, we can update the wieght based on distance, because the size of cells is known. If the optimal problem was to reduce the tortuosity then we can update weights based on changing the path from horizontal to vertical edges. The information about changing the direction is included in graph adjacency. The drawback of Cell-decomposition algorithm is the number of generated cells for high dimension, computation cost, and this algorithm does not work in dynamic environments. The idea of dividing the space into manageable sections is presented in many researches [7] using different techniques. In [10] the authors review some cells decomposition methods i.e approximate cell decomposition which based on drawing a mesh grid in the space, then exclude the cells on obstacle areas. This method is efficient but it generates large number of cells, and it is not complete in all cases. The seconds method is to generate all the cells based on obstacles edges; they consider each edge like a line and then find the intersections with others edges or cells. The next improvement for CD was by dividing the space quarterly, and tests every cell, if it has obstacle then we re-divide it quarterly again. This method is easy to implement. And we can control the minimum size of generated cell. 4 Problem formulation and proposed solution As we mentioned before the RRTs as example of randomize algorithm is good in high dimension or continuous spaces, and gives better result than typical graph-based algorithms. In general the problem with these algorithms is planning in small areas. In cell-decomposition case, it is efficient in low dimensions planning (relatively small graphs), but the building of the graph could be hard, or computational consuming when the environment is more complicated or the dimension is high. The other cutting edge between these two methods is that RRTs can work in dynamic and continuous environment while original CD algorithm cannot. We try to introduce and use the available spatial information into randomize planner to overcome the drawback of these two types of planners. 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014 275

We try to make a tradeoff between these two types of planners, or create a hybrid planner. In other words in high dimension, uncertainty environments we could use CD to produce a primitive path over 2D or 3D and provide this path to RRTs planner as bias-path. This will keep some reasonable balance between dynamic and uncertainties from one side, and optimality, efficiency in spatial planning from other side. Also CD can guide RRTs in small area which is the big disadvantage of RRTs planners. In order to show the difference between these two types of planners we make tests on two scenarios. First one is a simulation of offices and corridors architecture-schema and the other is the typical issue for RRTs which is small area and narrow passage. 5 Results We repeat the test 100 times for RRTs on every scenario and take the mean of the results for successful tries to reach the goal. In each time we setup RRTs planner to extending length ɛ = 0.5. and we consider that RRT fails to reach the goal after 3000, 5000, 10000, and 100000 tries of growing branches. We use Intel Xeon(R) PC with CPU of 2.67 GHz, and 6 GB of memory, and Windows 7 64-bit. We implement the algorithm in MATLAB environment. Figure 4: Building-like scenario, a: Using RRTs to find the path, b: Using CD to find a path, c: Extracted calls from the space The CD planner use Dijkstra algorithm for search on the graph. The Dijkstra in this case has O(log(N) E) time complexity. Where N is the nodes number and E is the edges number in the graph. In the first case we try to simulate path planning in a building-like scenario. The results in Table 1 show that CD algorithm is faster than RRTs in complete case by 7.6032 times. Fig. 4 shows this scenario which consists of rooms and corridors. The first figure Fig. 4(a) is using RRTs planner to find a solution. And Table 1 shows that the algorithm is probabilistically complete at iteration < 10000 times. In Fig. 4(b) we use CD planner to find a path between two positions which lay on cell 3 and 12 as shown is Fig. 4(c). Table 1: Test results of building-like scenario Nodes Preparing Planning Total time Path Time when Successful Time Time when success Length fail CELL Dec. 33 0.3152 0.0056(1) 0.3208(1) 40.92-100 % RRTs(3000) 478.17 0 1.8538(331.03) 1.8538(5.77) 43.46 2.0520 12% RRTs(5000) 547.17 0 2.4594(493.17) 2.4594(7.67) 41.77 3.2539 90% RRTs(10000) 540.48 0 2.4391(435.55) 2.4391(7.60) 42.71-100% The results in Table 1 show the nodes number of CD and RRT Trees. In CD case the nodes number represents graph-nodes which is constant. However, in RRTs case the node numbers is taken as average of 100 times repeating the algorithm. Each time when the algorithm is executed, we set the planner to make 3000, 5000, 10000 tries to add a branch to tree. The preparing time is the time required to calculate the graph in CD case. It not required in RRTs as we mentioned above. We can see from planning time the efficiency of CD compared to RRTs in 2D and non-holonomic objects. From this number we can see that for repeated task the CD can be 436 times faster for repeated task than RRTs and the graph size is constant which makes it applicable for real-time planning. In second scenario we try to simulate narrow passage which is typical test for RRTs. The results in Table 2 show that after 100000 iteration RRTs is not complete yet. Also the tree points block the tree from extending 276 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014

the points taken in narrow passage. It means when tree-point is very near to side of passage gate and we try to branch from this point, the edge to a new point is in collision. Table 2: Test results of small and narrow passage scenario Nodes Preparing Planning Total time Path Time when Successful Time Time when success Length fail CELL Dec. 19 0.2 0.0027(1) 0.2027(1) 24.33-100% RRTs(3000) 554.65 0 1.1920(441.48) 1.1920(5.88) 22.14 2.1318 31% RRTs(5000) 629.66 0 1.5855(587.22) 1.5855(7.82) 23.17 3.2426 41% RRTs(10000) 644.9 0 1.8039(668.11) 1.8039(8.90) 23.08 5.8196 44% RRTs(100000) 729.07 0 6.3947(2368.40) 6.3947(43.90) 23.00 50.4409 67% This scenario is presented in Fig. 5 where figure (a) is the using of RRTs planner. The generated graph is shown in Fig. 5-b. In that figure we can see that the path is between cells 2 to 15. In Fig. 5-c path between two positions which lay on cell 2 and 15 and the generated cells. Figure 5: Narrow passage scenario, a: Using RRTs to find the path, b: Generated graph by CD algorithm, and c: Extracted calls from the space and the planned path In order to enhance RRTs planner, we try to pass the path points from spatial planner CD to RRTs as bias points (see Fig. 6), where the dots are points on CD path. We set the RRTs planner to make bias in probability of 0.2 to these points. The results are shown in Table 3. Table 3: Test results for two scenarios with bias to points on CD-path Nodes Preparing Planning Successful Successful with bias Time Time with bias with bias 1 s t scenario RRTs(3000) 428.58 1.8538 1.8189(-1.9%) 64% 12% RRTs(5000) 447.1 2.4594 2.0266(-17.5%) 100% 90% RRTs(10000) 463.31 2.4391 2.1459(-12%) 100% 100% 2 n d scenario RRTs(3000) 297.10 1.1920 0.5676(-52.4%) 86% 31% RRTs(5000) 292.72 1.5855 0.5512(-65.2%) 86% 41% RRTs(10000) 299.96 1.8039 0.6178(-65.8%) 86% 44% By comparison with previous results the bias enhance the completeness significantly to RRTs planner in all cases. Also in trap scenario the time of planning decreases about 52% in worth case while the completeness increases. We can see that success of the planner in narrow passage scenario using spatial guidance is stick to 86%. It is because of another drawback of RRTs which is branch-blocking. That means the extension distance for branch is long. The solution could be made by choosing smaller ɛ distance. However, it increases computation and memory cost, because the trees will contain larger number of nodes. 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014 277

Figure 6: planning using RRTs with bias to CD path points, dots are points on CD path 6 Conclusion In this paper we test CD which generates adjacency-graph and RRT planners in simple configuration spaces. The results show that using CD planner efficiently finds a path in static and known environments. CD is faster on preparing and planning a path in simple CSpace than RRTs planner. We test the idea of introducing the spatial information to RRTs planner and it gives a good result by improving the completeness and planning time. This work was a first step to build cognition hybrid planner which could work efficiently in continuous, high dimension space using the available knowledge and spatial information, and overcome the drawback of randomized sampling-base algorithms. The future work will focus on using available information to speed up complex motion planning process for robots in uncertainty and dynamic environments. We also think to optimize and reduce the length of the final path generated by CD planner. Acknowledgement: his work was supported by BUT IGA No. FSI-S-14-2533: Applied Computer Science and Control. References [1] Abbadi, A., Matousek, R.: RRTs Review and Statistical Analysis. International journal of mathematics and computer in simulation, (Issue 1, Volume 6, 2012), ISSN: 1998-0159, (2012) [2] Abbadi, A., Matousek, R., Jancik, S., Roupec, J.: Rapidly-exploring random trees: 3D planning. On International Conference on Soft Computing Mendel 2012, pp. 594-599, (2012) [3] Frazzoli, E.; Dahleh, M.; Feron, E.: Real-time motion planning for agile autonomous vehicles. American Control Conference, 2001. Proceedings of the 2001, vol.1, no., pp.43,49 vol.1, (2001) [4] Gulina, I.: Motion planning in challenging environments using rapidly-exploring random trees. Proceedings of International Conference on Soft Computing Mendel 2013, pp. 153-157, Brno (2013) [5] James, b., Veloso, M.: Real-Time Randomized Path Planning for Robot Navigation. In RoboCup 2002: Robot Soccer World Cup VI. Springer Berlin Heidelberg, (2003) [6] Kuffner, J., LaValle, S.,: RRT-connect: An efficient approach to single-query path planning, Robotics and Automation. 2000. Proceedings. ICRA 00. IEEE International Conference on vol.2, no., pp.995,1001 vol.2, (2000) [7] Latombe, J.: Robot Motion Planning. Kluwer Academic; Book, (1991) [8] Manubens, M., Devaurs, D., Ros, L., Corts, J.: Motion Planning for 6-D Manipulation with Aerial Towedcable Systems. Proceedings of Robotics: Science and Systems, Berlin (2013) [9] Militao, F., Naden, K., Toninho, B.: Improving RRT with Context Sensitivity, April 13, (2010) [10] Sleumer, N., Tschichold-Grman, N.: Exact Cell Decomposition of Arrangements used for Path Planning in Robotics, (1999) [11] Urmson, C., Simmons, R.: Approaches for heuristically biasing RRT growth. Intelligent Robots and Systems, 2003. (IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, vol.2, no., pp.1178, 1183 vol.2, 27-31 Oct. (2003) 278 20th International Conference on Soft Computing MENDEL 2014, Brno, Czech Republic, June 25-27, 2014