Nearest Neighborhood search in Motion Planning

Transcription

1 Nearest Neighborhood search in Motion Planning Lakshmi Reddy B, Xiabing Xu and Nancy M. Amato Computer Science Department Texas A&M University {xiabing, Abstract Nearest Neighbor Search plays a vital role in building road-maps in Motion Planning. These are implemented in any Road-map building method such as PRM, OBPRM, Gauss PRM, etc. Motion Planning is the science of designing a successfully implementable plan in any Configuration Space (often called C-space) in order to move the Robot from a given Start Configuration to a Goal Configuration by avoiding collisions with obstacles lying in the C-space. Obstacles can be either static or dynamic. In this process of building Road-maps, Node-generator randomly generate the nodes throughout the C-space wherein each node represents the possible position of the Robot that might be attained while in motion. From all the above nodes generated by Node-generator, Road-map stores the portion of the nodes that are free from possible collisions which is determined by Collision-detection-test. One such Collisiondetection-test that is currently used is RAPID. The nodes stored in the Road-map are connected using Local-planner such as Straight-line, Rotate AT S, etc. Nearest Neighborhood Search problem comes here where this paper focused towards after serious investigation of various environments and each environment was investigated by varying number of Nearestneighbors(K), number of Nodes(N ) and Epsilon ǫ. Investigation into various environments by variation of K, N and ǫ over a wide range can help in finding suitable K, N and ǫ values that could optimize the total time to build the Road-map with out loosing realiability and quality of Road-map. Index Terms Nearest-Neighbor, Brute-Force, CGAL, MPNN, Road-map, ǫ, K, N and Graphs. I. GLOSSARY Node- Vertices in the Configuration Space correspond to Collision free Configurations of the Robot in a Work Space or Configuration Space. Node Generation- Node generation strategies are methods used to generate nodes(robot Configurations) randomly throughout the C-space. Collision Detection- Collision Detection test used to select collision free Robot configurations to be used as nodes in the Roadmap. K- K denotes the maximum number of nodes that to be connected by a query node to it s neighborhood nodes. N - N denotes the maximum number of collision-free nodes that can be generated randomly throughout the C-space by a Node-generator. Epsilon(ǫ)- ǫ uses to extend the proximity of the query node by (1+ǫ)in order to search nearest neighbors by query node. Local Planner- Planner which connects the two nodes by an edge if a path between two corresponding configurations or nodes that can be found by local planning method. Queries- Queries are processed by connecting the initial and goal configurations to the Roadmap and then finding the path in the roadmap between these two connection points. Distance Metric- are used to calculate the distance between the nodes one should try to connect when building the Roadmap. Total Time- is the total time taken to generate the nodes and to find the nearest neighborhood nodes for given K in a Work space or Configuration Space. Average total time- is the time taken by the query node to find its nearest node and mathematically it is [Totaltime/(N K)]. Brute-Force (BF)- BF is the Exact Neighborhood Finder which uses sorting methods to find nearest neighbors. CGAL- CGAL used to find approximate nearest neighborhood by building single Kd-tree throughout the C-space in order to find nearest neighbors. MPNN- MPNN library also used to find approximate nearest neighborhood by building multiple small Kd-tree throughout the C-space in order to find nearest neighbors. II. INTRODUCTION Nearest Neighborhood search is one of the fundamental problems in Motion planning. The motion planning problem can be defined as finding a collision free path between a start and goal configurations of a robot in a given C-space with obstacles. These robot configurations, or positions are represented by a set of parameters that describe the position and orientation of the robot; these parameters are often referred to as the robots degrees of freedom (DOF). Over the past two decades, many different researchers have studied sampling-based motion planning techniques such as the Probabilistic Road-map Method(PRM). This has led to many variants, each with its own merits. It is difficult to compare the different techniques because they were tested on different types of environments, using different underlying libraries, implemented by different people on different machines. We have provided a comparative study and analysis of a number of these techniques, all implemented in a single system and run on the same test environments and on the same computer. We encountered many difficulties and pitfalls during this study. We will identify them and discuss our solutions based on our experimental research. Assume the robot that can use to clean the house. This robot will have to be able to perform many different tasks.

2 One of the most important of such tasks is to be able to navigate from an initial position to a final one. Moreover, to save time resources, it is strongly recommended that the robot will navigate through the shortest path. In technical terms robot will navigate through the shortest path by connecting the nearest neighbors between initial and final configurations in a given C-space. This paper dealing with this problem of finding K nearest neighbors for all N in C-space in order to build effective and quality road-map. Like house, many other real world domains are populated by various static and dynamic physical objects like walls, doors, tables, animals and more. Since all these objects prevent the robot from moving through them, they are considered to be obstacles to the robot moving. Thus, the robot should take into account all these obstacles while planning its own motions in order to prevent collisions. This problem is called the basic motion-planning problem. Motion planning arises not only in robotics but in many other areas such as intelligent CAD (virtual prototyping), mixed reality systems (training and computer-assisted operation), and even computational biology and chemistry (protein folding and drug design). This paper concentrated on the following problem. Given an initial configurations and orientation and a goal configurations and orientation, generate a path specifying a continuous sequence of configurations and orientations by connecting initial configuration(say I) to its nearest neighbor(say n 1 ), then connecting n 1 to its nearest neighbor(say n 2 ) with suitable orientation and so on up to goal configuration of the robot while avoiding collisions with the obstacles, more formally, this paper focused towards the problem of finding K number of nearest neighbors for all K starting at the initial configurations and orientation, and terminating at the goal configuration and orientation. For simplicity, all the obstacles are static, and the domain has two dimensions. Usually many researchers assume K, K and ǫ using some heuristic techniques. In this paper, after serious investigation of various environments using Brute-Force, CGAL and MPNN nearest neighborhood finders, we concluded suitable values for K, N and ǫ to optimize total time taken to find nearest neighbors and show the results. First we give a general description of the system and explain some of the implementation aspects. Then, we discuss a few algorithmic aspects of the system. Finally, a few possible improvements are suggested, and some of the experimental results are shown. III. PRELIMINARIES HOOK: Hook robot is a rectangular rigid body bent in to five directions perpendicularly and Hook-Rigid environment is comparatively simple environment looks like rectangular box divided in to 3 chambers by 2 walls having rectangular openings on each of the walls as shown in the Fig-4. MAZE: Maze resembles a Torus with a shaft fitted between the gap and the entire environment is similar to a Cuboid with two holes on opposite sides and various inter-connected narrow passages connecting the holes of the Cuboids inside; some of the passages have a dead-end as shown in the Fig-9. The time is ripe to introduce my Research Topic which is called the K-nearest neighbor search. Naturally one can wander about why so much research work has been going on in finding the nearest neighbor and building road maps? This seems to be an apparently simple topic. But when encountered the Maze environment you will realize that you were looking in the wrong direction and it is not as easy building a simple Road map. Generating collision free configurations in narrow passages needs more attention in the Maze environment since it is quite difficult to locate the nearest neighbors in these narrow passages without encountering obstacles or dead-ends. CLUTTER:Cluttered environment resembles a real-time environment consists obstacles in between. These environments are quite complex and it is comparatively more difficult to design road maps compared to the Hook and Maze environments. This is because these environments contains obstacles and can be divided into three sub-environments depending on the number of obstacles i.e. Least Cluttered(Fig- 15), Medium Cluttered (Fig-16), Most Cluttered (Fig-17). Unlike Hook and Maze, Cluttered environment is open environment and dimension of the environment changes according to change in start and goal configurations. The environment has been getting complicated as it is difficult to make a path on increasing the number of obstacles and it might not be able to process all queries for all start and goal configurations as the target hides in an environment cluttered with obstacles. As we increase number of obstacles, difficulty to generate collision free configurations increases drastically. For example to generate collision free configurations for least, medium, and most cluttered environments, node generator has to generate 22494, 77554, and respectively where remaining configurations will filter out by Collision Detection test. Kd-tree:Kd-tree is an algorithm used in CGAL and MPNN in order to search nearest neighbors for all nodes N and for given K in a given C-space. Kd-tree algorithm used in CGAL and MPNN searches nearest neighbors comparatively very faster than the Sorting algorithms used in Brute-Force Neighborhood Finder and it is experimentally proved after serious investigation of various environments presented in this paper. Kd-tree can construct using recursive procedure described below. This procedure has two parameters: a set of points and an integer. The first parameter is set for building desired Kd-tree. Initially this is the set P. The second parameter is the depth of the root of the subtree that the recursive call constructs. The depth parameter is ZERO at first call. The depth is important because it determines whether we must split C-space with a vertical or a horizontal line. The procedure returns the root of the Kd-tree. At the root we split the set P with a vertical line in to two subsets of roughly equal size. The splitting line is stored at

3 the root. P left the subset of points to left is stored in the left subtree, and P right, the subset of points to right is stored in the right subtree. At the left child of the root we split the P left into two subsets with a horizontal line. The points below or on it are stored in the left subtree, and the points above are stored in right subtree. The left child itself store the splitting line. Similarly P right is split with a horizontal line, which are stored in the left and right subtree of the right child. At the grandchildren of the root, we split again with a vertical line. In general, we split with a vertical line at nodes whose depth is even, and we split with horizontal line whose depth is odd. Fig-1 illustrates how the splitting is done and what the corresponding Kd-tree looks like and the algorithm for building Kd-tree described below[4]. Fig. 1. Above Figure illustrates how the splitting is done and the corresponding Binary tree. Algorithm BUILDKDTREE(P,depth) Input: A set of points P and the current depth. Output: The root of the kd-tree storing P. 1) if P contains only one point 2) then return a leaf storing this point 3) else if depth is even 4) then Split P into two subsets with a vertical line through the median X-coordinate of the points in P.Let P 1 be the set of points to the left of line or on line, and let P 2 be the set of points to the right of line. 5) else Split P into two subsets with a horizontal line through the median Y-coordinate of the points in P.Let P 1 be the set of points to the below of l or on l, and let P 2 be the set of points above l. 6) v left BUILDKDTREE(P 1, depth +1). 7) v right BUILDKDTREE(P 2, depth +1). 8) Create a node v storing l, make v left the left child of v, and make v right the right child of v. 9) return v. A Kd-tree for a set of N points uses Θ(N ) storage and can be constructed in Θ(N logn ) time. BRUTE FORCE:Brute-force is the exact nearest neighborhood finder which uses to find nearest neighbors in motion planning using sorting algorithms. Mainly two kinds of sorting algorithms used in Brute-force that are very much similar to the selection sort and insertion sort discussed below. First sorting method works by finding the nearest neighbor using a linear scan, calculating distance using Distance-metric and swapping it into the first position in the list, then finding the second nearest neighbor by scanning the remaining elements, and so on up to the given K value. This sorting method is unique compared to almost any other algorithm in that its running time is not affected by the prior ordering of the list: it performs the same number of operations because of its simple structure. This method requires (n - 1) swaps and hence Θ(n) memory writes. However, this sorting method requires (n - 1) + (n - 2) = n(n - 1) / 2 = Θ(n 2 ) comparisons. Thus it can be very attractive if writes are the most expensive operation, but otherwise this sorting method will usually be outperformed by other sorting algorithm described below or the more complicated algorithms. Time complexity = Θ(N 2 ) and space complexity = Θ(N ) for N number of nodes. Second sorting method is a simple sorting algorithm that is relatively efficient for small lists and mostly-sorted lists, and often is used as part of more sophisticated algorithms. It works by calculating distance of the nodes using Distance-metric one by one and inserting them in their correct position into a new sorted list of length K. In arrays, the new list and the remaining elements can share the array s space, but insertion is expensive, requiring shifting all following elements over by one. It inserts each item into its proper place in the final list. The simplest implementation of this requires two list structures - the source list and the list into which sorted nearest neighbor nodes are inserted. To save memory, most implementations use an in-place sort that works by moving the current item past the already sorted items and repeatedly swapping it with the preceding item until it is in place. For this method also Time complexity = Θ(N 2 ) and space complexity = Θ(N ) for N number of nodes. CGAL: Computational Geometry Algorithms Library In this paper CGAL used to find nearest neighbors. It searches nearest neighbors in a C-space by building single Kd-tree, as we discussed above, throughout the C-space. While Brute-Force finds exact nearest neighbors using sorting algorithms, CGAL is approximate nearest neighborhood finder and finds approximate nearest neighbors by varying ǫ. As ǫ increases, total-time taken to find nearest neighbors decreases, which is our main objective to attain but in parallel reliability of the road-map also decrease and may not process complex queries effectively. As it builds single Kd-tree throughout the C-space, it may not work for all cases. For example, C-space shown below. In the Fig-2 below, Kd-tree may not spread in to the areas surrounded by the obstacles and in turn unable to process queries in these regions. MPNN:Motion Planning Nearest Neighbor Library We have implemented our NN algorithm in C++ as part of the new library, MPNN, for NN searching in the context of motion planning. This library is publicly available [6]. The kd-tree implementation that we used in MPNN is slightly different than in CGAL. In CGAL we build single Kd-tree throughout the C-space but in MPNN we build multiple

4 Fig. 2. Work-space and corresponding C-space (colured polygons are obstacles in the C-space.) Kd-trees depending on N and obstacles present in the C-space. It is natural to ask whether building multiple Kd-trees might be even better. This is particularly helpful for problems like the double bug trap in Figure 1. New trees can be grown from parts of C-space that are difficult to reach. Controlling the number of trees and determining when to attempt connections between them is difficult. Some interesting recent work has been done in this direction [6, 7]. These additional trees could be started at arbitrary (possibly random) configurations. As more trees are considered, a complicated decision problem arises. The computation time must be divided between attempting to explore the space and attempting to connect trees to each other. It is also not clear which connections should be attempted. Many research issues remain in the development of this. A limiting case would be to start a new tree from every sample in Fig-2 and to try to connect nearby trees whenever possible. This approach results in a graph that covers the space in a nice way that is independent of the query. We then used MPNN in implementations of PRM-based planners in the Motion Strategy Library (MSL) [5]. This approach also may not feasible for all cases. Especially, this happened within the MPNN Neighborhood Finder when a higher number of near neighbors compared to the available data points was being requested i.e. value of K is greater than the size of the Kd-tree. This is the Bug that is inherent in the MPNN Neighborhood Finder Algorithm and currently research work has been focused towards to fix this bug. PRM: PROBABILISTIC ROADMAP METHOD PRM is the method of building Roadmaps using different techniques. Basically it divided in to two phases, Construction phase and Query phase as shown in the algorithm below[1]. Step-1: PREPROCESSING: ROADMAP CONSTRUCTION 1) NODE GENERATION (find collision-free configurations) 2) CONNECTION (connect nodes to form roadmap) distance metric selects candidate node pairs local planner checks connection 3) ROADMAP ENHANCEMENT (connectivity/quality) e.g., generate more nodes, use smart local planner Step-2: QUERY PROCESSING 1) CONNECT START AND GOAL TO ROADMAP distance metric selects roadmap nodes Use StraightLine, or RotateATS, or Combination of both StraightLine and RotateATS local planners. 2) FIND PATH IN ROADMAP BETWEEN CONNECTION NODES. OBPRM: OBSTACLE BASED ROAD MAP METHOD -These method is also more or less similar to the PRM but OBPRM generates more free configurations at obstacle surface which makes easy to construct the Roadmap between the obstacles. Whereas a PRM generates nodes randomly distributed throughout the C-space. Since space between the obstacles represents only a small fraction of C-space, the probability of nodes being generated there is proportionally small. Speed-up: The term speed-up implies, how faster the neighborhood finders CGAL and MPNN calculate its neighborhoods than the Base neighborhood Brute-Force and mathematically it defines as Speed-up factor = (Base Total-time/NF Total-time) = (Average Base Total-time/Average NF Total-time) Since Average Total-time = (Total-time/N K) IV. EXPERIMENTS A. Chossing appropriate values for K and N It is very important to choose appropriate values for K and N in order to optimize the Total-time taken to find nearest neighbors for all nodes. This in turn it helps building effective and quality road-map for a given Start and Goal positions. To determine values for K and N, investigations were carried out in both Hook and Maze in order to apply the same values for all other environments like Cluttered, Narrow-Passage, etc. The initial range of values for K and N were chosen after discussions with the teamand work was commenced on Hook and Maze in order to find appropriate values for K and N. Chosen values for K included 1, 2, 4, 8, 16, 32, 64, and 128. Chosen values for N included 1000, 2000, 5000, 10000, 15000, and and constant ǫ value of 0.0 was chosen. Iterations were commenced by keeping N constant, varying K on X-axis and Total-time on Y-axis; this was followed by keeping K constant, varying N on X-axis and Total-time on Y-axis. Finally Total-time and Average-Total-time for all Neighborhood finder methods for both Hook & Maze were calculated and tabulated. B. Reasons for choosing Hook and Maze Hook and Maze were chosen for specific reasons: the Node generator for Hook generates approximately 1.5 times the

5 nodes that are stored in the Road-map and is relatively simple; on the other hand Maze is a complex environment as Node-generator generates approximately 5 times the nodes stored in the Road-map in order to find nearest neighbors respectively. For example, to generate 5000 collision free nodes in the Hook and Maze environments, the Node generator will actually generate 7889 and nodes respectively where the Collision-detector RAPID filter out the remaining. These environments are explained in detail later and complexity of Hook and Maze is shown in Fig-3 Fig. 4. (a)hook-rigid and its Environment, (b)start and Goal Configurations of Hook. Fig. 3. Above Figure illustrates Complexity of Hook and Maze. C. System Configuration As discussed above, We have provided a comparative study and analysis of different environments, all implemented in a single system and run on the same test environments and on the same computer. 1) Operating System: Linux 2) Compiler: Emacs 3) System name: Big-Spring, consists of 96 CPUs which subdivided into 24 components where each single component called as COMPUTE. Configuration of each COMPUTE is given below: Hardware: CPUs: Ghz, Memory(RAM): 3.96 GB Software: Os(x86) 4) Total Disk-size of Big-Spring is GB. A. Hook V. ENVIRONMENTS 1) Hook: Hook robot is a rectangular rigid body bent in to five directions perpendicularly as shown in the Fig 2(a) and Hook-Rigid environment is comparatively simple environment looks like rectangular box divided in to 3 chambers by 2 walls having rectangular opening on each of the walls as shown in the Fig 2(b). 2) Parameters used for Hook:: Dimensions of the Hook environment: Min X= -100 and Max X=100 Min Y= -100 and Max Y=100 Min Z= -400 and Max Z=200 Start configuration:(0,0,120, 0,0,0) Goal position:(0,0,-280, 0,0,0) Node generation method: BasicPRM Neighborhood Finder: Brute-Force, CGAL, MPNN Local Planner: Straight Line and Rotate AT S. Distance Metric: scaledeuclidean Collision Detection: RAPID Position resolution= 5.5 Orientation resolution= 0.05 Epsilon= 0.0 Nodes: 1000, 2000, 5000, 10000, and K closest: 1, 2, 4, 8, 16, 32, 64 and 128 With the above parameters Hook environment has been investigated and tabulated below: 3) Tables: Total-time: is the total time taken by the N number of nodes to find its K nearest neighbors for each of the nodes in a given C-space. Average Total-time: is the average time taken by the single node to find its nearest neighbor in the C-space. Mathematically it defined as (Total-time/N K)

6 Total-time of Hook for varying K and N 0.9 K BF Total-time CGAL Total-timeMPNN Total-time TABLE I TOTAL-TIME FOR N =1000, 2000, K BF Total-time CGAL Total-time MPNN Total-time TABLE II TOTAL-TIME FOR N =10000, 15000, Fig. 5. (a) Total-time for Brute-Force for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for Brute-Force for all K=1, 2, 4, 8, 16, 32, 64 and 128 Explanation: Observations: In some tables there are apparent blank spaces. This happened within the MPNN Neighborhood Finder when a higher number of near neighbors compared to the available data points was being requested i.e. value of K is greater than the size of the Kd-tree. This is the Bug that is inherent in the MPNN Neighborhood Finder Algorithm and currently research work has been focussed towards to fix this bug. From above tables, we can say that the MPNN Neighborhood Finder may not work for all K especially where K greater than 8 as aforementioned. As discussed in Brute-Force, it is necessary to calculate distance from every node to every other node for all N. This increases complexity at N 2.Therefore on increasing N the Total-time increases rapidly. Since the dimensions of the environment remain constant throughout the experiments, the environment becoming congested if there is an increase in N. It is also seen that above a certain value, although if K is increased, Total-time may not increase as expected. 4) Plotting: After tabulating results, it was difficult in choosing important parameters for plotting. However, after discussions with the team, it was decided to plot two types of graphs by taking K on X-axis and Time on Y-axis for constant N, and N on X-axis and Time on Y-axis for constant K in order to find appropriate values for N and K respectively. Graphs of Hook for Total-time, varying K and N

7 Explanation: Fig. 6. (a) Total-time for CGAL for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for CGAL for all K=1, 2, 4, 8, 16, 32, 64 and 128 Explanation: As discussed in CGAL, it is necessary to search for nearest neighbors in Kd-tree for given K for all N. The complexity correspondingly increases in the order of K log N. Therefore on increasing N, the Total-time will not increase as rapidly as expected because of logn factor. Since the logarithm function increases slowly, total time also increases slowly and for small values of K, the difference between the Total-time taken for N =1000 and Total-time taken for N =20000 is very low. Unlike in CGAL, MPNN builds several small Kd-trees throughout the C-space. If the requested value of K is larger than the nodes present in that Kd-tree then MPNN will not work for that case and returns error. For the same reason, in the above plot, graphs were terminated at different K-values for different N. As discussed in MPNN, it is necessary to search for nearest neighbor in the specific Kd-tree where the query node lies for given K for all N. Thus the complexity increases in the order of N log(n )/X where X denotes number of Kd-trees built in the C-space. The same reason explains a higher rate of Total-time increases as in CGAL but at higher rate than the CGAL. Observations: For Brute-Force, as N increases, Total-time increases rapidly. For CGAL, as N increases, Total-time increases logarithmically (at very low rate). For MPNN, as N increases, Total-time increases as in CGAL but with higher rate than CGAL. From above plots it can be inferred that for higher values of N, behavior of the graphs is Dynamic and for lower values it is almost Static. So it is better to take medium values for better results. From above plots it can be said that above some K, the environment becomes congested and there is a minimal effect on Total-time on increasing K. To generate N number of nodes in the Hook environment, neighborhood finder actually generates approximately (1.5) N number of nodes where collision detector filter out the remaining. 5) Speed-up: The term speed-up implies, how fast the neighborhood finders CGAL and MPNN calculate its neighborhoods when compared to the Base neighborhood Brute-Force. It can be mathematically defined as the following Speed-up factor = (Base Total-time/NF Total-time) Fig. 7. (a) Total-time for MPNN for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for MPNN for all K=1, 2, 4, 8, 16, 32, 64 and 128 = (Average Base Total-time/Average NF Total-time) Since Average Total-time = (Total-time/N K)

8 Speed-up plots for CGAL and MPNN comparing with Brute-Force of Hook: Fig. 8. Speed-up plots for CGAL(red) and MPNN(green). Fig. 9. Maze. (a)maze and its Environment, (b)start and Goal Configurations of observations: CGAL is fastest Neighborhood-Finder followed by MPNN and then followed by Brute-Force. Since Brute-Force is exact Neighborhood-Finder it is more reliable than CGAL and MPNN. B. Maze 1) Maze: Maze resembles a Torus with a shaft fitted between the gap and the entire environment is similar to a Cuboid with two holes on opposite sides and various inter-connected narrow passages connecting the holes of the Cuboids inside; some of the passages have a dead-end as shown in the fig-3(a). The time is ripe to introduce my Research Topic which is called the K-nearest neighbor search. Naturally one can wander about why so much research work has been going on in finding the nearest neighbor and building road maps? This seems to be an apparently simple topic. But when encountered the Maze environment you will realize that you were looking in the wrong direction and it is not as easy building a simple Road map. Generating collision free configurations in narrow passages needs more attention in the Maze environment since it is quite difficult to locate the nearest neighbors in these narrow passages without encountering obstacles or dead-ends. 2) Parameters used for Maze:: Dimensions of the Maze environment: Min X= -8 and Max X=7 Max Y=16.5 Max Z=11 Start position:(0,0,15, 0,0,0) Goal position:(0,0,-15, 0,0,0) Node generation method: BasicPRM Min Y= and Min Z= -9 and Neighborhood Finder: Brute-Force, CGAL, MPNN Local Planner: Straight Line and Rotate AT S. Distance Metric: scaledeuclidean Collision Detection: RAPID Position resolution= 0.12 Orientation resolution= 0.05 Epsilon= 0.0 Nodes(N ): 1000, 2000, 5000, 10000, and K closest: 1, 2, 4, 8, 16, 32, 64 and 128 With the above parameters Maze environment has been investigated and tabulated below: 3) Tables: Total-time: is the total time taken by the N number of nodes to find its K nearest neighbors for each of the nodes in a given C-space. Average Total-time: is the average time taken by the single node to find its nearest neighbor in the C-space. Mathematically it defined as (Total-time/N K)

9 Total-time of Maze for varying K and N 0.9 K BF Total-time CGAL Total-timeMPNN Total-time TABLE III TOTAL-TIME FOR N =1000, 2000, K BF Total-time CGAL Total-time MPNN Total-time Observations: TABLE IV TOTAL-TIME FOR N =10000, 15000, In some tables there are apparent blank spaces. This happened within the MPNN Neighborhood Finder when a higher number of near neighbors compared to the available data points was being requested i.e. value of K is greater than the size of the Kd-tree. This is the Bug that is inherent in the MPNN Neighborhood Finder Algorithm and currently research work has been focussed towards to fix this bug. From above tables, we can say that the MPNN Neighborhood Finder may not work for all K especially where K greater than 8 as aforementioned. 4) Plotting: After making tables, Its the time for plotting but this time we dont find any difficulty in selecting parameters as already plotted for Hook but this time it will be so anxious to see the behavior of the graphs whether they come in the same manner as Hook or it has its own style. As plotted for the Hook, Maze graphs are also plotted by taking K on X-axis and Time on Y-axis for constant N, and N on X-axis and Time on Y-axis for constant K in order to find appropriate values for N and K respectively. Fig. 10. (a) Total-time for BF for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for BF for all K=1, 2, 4, 8, 16, 32, 64 and 128 Explanation: Since the Maze environment is comparatively complex than Hook, the Total-time taken to find nearest neighbors also comparatively higher than that of Hook. Since the environment itself having narrow-passages, the environment become congested even for small values of N and even finding neighborhoods in those narrow-passages is difficult task, so for increasing K will not affect on Total-time as expected and difference in Total-time becomes very low for higher values of K. As discussed in Brute-Force, calculating distance from every node to every other node for all N in turn increases the complexity at the order of N 2.So if we increase N the Total-time increases rapidly.

10 Explanation: Fig. 11. (a) Total-time for CGAL for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for CGAL for all K=1, 2, 4, 8, 16, 32, 64 and 128 Explanation: Since the Maze environment is complex than the Hook, building Kd-tree in the narrow-passages becomes difficult and so Total-time taken to find nearest neighbors is also increases than the Hook environment. Behavior of the graphs is very much similar to the Hook environment. As discussed in CGAL, searching for nearest neighbors in Kd-tree for given K for all N increases complexity at the order of N logn. So, although if we increase N, the Total-time will increase steadily because of log(n ) factor. Since the logarithm function increase very slowly, total time also increases very slowly and for small values of K, the difference between the Total-time taken for N=1000 and Total-time taken for N=20000 is very low. Fig. 12. (a) Total-time for MPNN for all N =1000, 2000, 5000, 10000, and 20000, (b) Total-time for MPNN for all K=1, 2, 4, 8, 16, 32, 64 and 128 Since the environment is comparatively complex than the Hook and also environment consists of numerous narrow-passages, we need to build more Kd-trees in order to build effective Road-map. Thus Total-time taken to find nearest neighbors in Maze is almost double than the Total-time taken to find nearest neighbors in Hook using MPNN. Since environment consist narrow passages and more Kd-trees, if we request more nearest neighbors than nodes present in that Kd-tree then it will give error and for the same graphs above are terminated at different K for different N. As we discussed in MPNN, we have to search for nearest neighbor in the specific Kd-tree where the query node lies for given K for all N. Thus the complexity increases at the order of N log(n )/X where X denotes number of Kd-trees built in the C-space and for the same reason Total-time increases as in CGAL but at higher rate than the CGAL. Observations: If environment is complex then the Total-time taken to find nearest neighbors increase for all Brute-Force, CGAL, and MPNN neighborhood finder methods. For Maze, to generate N number of nodes neighborhood finder actually generates approximately 5 N number of nodes and collision detector filter out the remaining. For Brute-Force and CGAL, Total-time taken to find nearest neighbors in Maze is 1.25 times higher than the Total-time taken in Hook. For MPNN, Total-time taken to find nearest neighbors in Maze is 2 times or higher than the Total-time taken in Hook. For Brute-Force, as we increasing N, Total-time increases rapidly. For CGAL, as we increasing N, Total-time increases logarithmically but with very slow phase. For MPNN, as we increasing N, Total-time increases as in CGAL but with higher rate than CGAL. From above plots we can say that, for higher values of N, behavior of the graphs is Dynamic and for lower values it is almost Static. So it is better to take medium values for better results. From above plots we can say that, at above some K, the environment become congested and there would be very minimum effect on Total-time and looks almost constant as we increasing K. 5) Speed-up: The term speed-up implies, how faster the neighborhood finders CGAL and MPNN calculate its neighborhoods than the Base neighborhood Brute-Force and mathematically it defines as Speed-up factor = (Base Total-time/NF Total-time) = (Average Base Total-time/Average NF Total-time) Since Average Total-time = (Total-time/N K)

11 Speed-up plots for CGAL and MPNN comparing with Brute-Force of Hook: and high quality Road-map as nodes may deploy well throughout the C-space if dimension of the environment is large. VII. CHOOSING EPSILON(ǫ) Fig. 13. Speed-up plots for CGAL(red) and MPNN(green). VI. CHOOSING APPROPRIATE VALUES FOR K AND N Hook and Maze environments are investigated and the time is ripe to choose appropriate values for K and N. A. Choosing K-Value For higher values of K, MPNN may not work properly in case if K greater than the nodes present in that Kd-tree. For lower values of K, Road-map may not be efficient. We might not be able to process complicated queries. Since the dimensions of the environment are constant, for higher value of K, environment may become congested and processing time may increase. Feasible value of K should work for all Neighborhood Finder methods and also for all N. So, feasible value for K is 8 for Brute-Force, CGAL, and MPNN Neighborhood Finders as it works for all Neighborhood Finders for all values of N. B. Choosing N -Value Since the dimensions of the environment are constant, for higher values of N, environment may become congested and it will not yield expected results in some of the cases and sometimes it causes waste of time and also memory. For lower values of N, nodes might not be deploy well throughout the C-space and if nodes are not deployed well then Road-map will not be electively connected, quality of the Road-map will degrade and will not able to process complex queries. In case of MPNN, N should be greater than K at all Kd-trees, otherwise it will return error without processing the query. From the above graphs we can say that, for higher values of N, a graph looks dynamic and for lower values of N, graph looks almost static in behavior. From above, feasible value for N should be neither high nor low. So, feasible values for N are 5000 and but sometimes with N =5000 we may not build effective Now suitable values for K and N are obtained. For K=8 and N=10000, Total-time could be further optimized with suitable ǫ value. Therefore, by choosing appropriate value for ǫ we can further optimize the total-time taken to find nearest neighbors. Choosing ǫ is not as easy as it sounds; it requires more attention and investigation. As the range of ǫ value itself is very small (0 to 3), the reliability and quality of the road-map is highly sensitive to the value of ǫ and hence it is important to choose an appropriate ǫ value. So we need to investigate more environments resembling real-world domain. Therefore, the Cluttered environment along with Hook and Maze environments has been chosen to investigate in order to choose appropriate value for ǫ and discussed below. A. Clutter Cluttered environment resembles a real-time environment consisting obstacles in between. These environments are quite complex and it is more difficult to design road maps compared to the Hook and Maze environments. This is because these environments contain obstacles and can be divided into three sub-environments depending on the number of obstacles. The three sub-environments are Least Cluttered(Fig-15), Medium Cluttered(Fig-16), Most Cluttered(Fig-17). Unlike Hook and Maze, Cluttered environment is an open environment and dimensions of the environment changes according to change in start and goal configurations. The environment gets complicated as it is difficult to make a path as the number of obstacles increases and it might not be able to process all queries for all start and goal configurations as the target hides in an environment cluttered with obstacles. As we increase number of obstacles, difficulty to generate collision free configurations increases drastically. For example to generate collision free configurations for least, medium, and most cluttered environments, node generator has to generate 22494, 77554, and respectively where remaining configurations will be filtered out by collision detection test.

12 Epsilon BF-NF CGAL MPNN TABLE V TOTAL-TIME TAKEN BY BRUTE-FORCE, CGAL AND MPNN FOR HOOK Epsilon BF-NF CGAL MPNN TABLE VI Fig. 14. (a) Least Cluttered environment (b) Medium Cluttered environment (c) Most Cluttered environment with the Start(blue) and goal(dotted yellow) positions B. Parameters used for Clutter environment: Dimensions of the Clutter environment: Min X= -5 and Max X=5 Min Y= -4.5 and Max Y=5 Min Z= -10 and Max Z=10 Start position:(0,0,9, 0,0,0) Goal position:(0,0,-9, 0,0,0) Node generation method: BasicPRM Neighborhood Finder: Brute-Force, CGAL, MPNN Local Planner: Straight Line and Rotate AT S. Distance Metric: scaledeuclidean Collision Detection: RAPID Position resolution= , , and for least Clutered, medium Cluttered, and most Cluttered environments respectively. Orientation resolution= 0.05 for all three types of Cluttered environments. Nodes: K closest: 8 Epsilon: 0.0, 0.1, 0.2, 0.4, 0.8, 1.6 and 3.2. C. Tables: Investigations were carried out in order to choose appropriate ǫ value using Hook, Maze and Clutter environments with N =10000 and K=8, and data tabulated below TOTAL-TIME TAKEN BY BRUTE-FORCE, CGAL AND MPNN FOR MAZE Epsilon BF-NF CGAL MPNN TABLE VII TOTAL-TIME TAKEN BY BRUTE-FORCE, CGAL AND MPNN FOR LEAST-CLUTTERED Epsilon BF-NF CGAL MPNN TABLE VIII TOTAL-TIME TAKEN BY BRUTE-FORCE, CGAL AND MPNN FOR MEDIUM-CLUTTERED Epsilon BF-NF CGAL MPNN TABLE IX TOTAL-TIME TAKEN BY BRUTE-FORCE, CGAL AND MPNN FOR MOST-CLUTTERED

13 Observations: Total-time is not varying for Brute-Force. It is almost constant at all values of ǫ and it seems ǫ doesn t effect the Brute-Force as shown in the Fig. This is because Brute-Force uses sorting algorithms to find nearest neighbors. while Using sorting algorithms the distance(s) from every node to all other nodes is (are) calculated for all N using Distance-metric formula. Then all the configurations are sorted out according to the distance calculated and stored in the road-map for given K as explained above). Thus, ǫ doesn t effect the Brute-Force neighborhood finder. Total-time is strictly decreasing for CGAL and MPNN as the ǫ value increases. This is because CGAL and MPNN use Kd-tree algorithm to find nearest neighbors. As the ǫ value increases, proximity of the query node extends by (1+ǫ) to find its nearest neighbors in Kd-tree which facilitates building road-map faster. As ǫ value increases, query node will try to connect the neighbors which are farther than the exact nearest neighborhoods, Total-time decreases but in parallel the reliability and quality of the road-map will degrade as some of the configurations will be discarded and the count increase for higher value of ǫ. Therefore, ǫ value should not be high. Fig. 16. Total-time obtained for CGAL for all environments by varying ǫ. Explanation: CGAL build single Kd-tree throughout the C-space. As ǫ increases, proximity of the query node also increases in the Kd-tree. Query node should be connected to the K number of nodes and remaining are discarded in its neighborhood. Hence number of discarded nodes increases as ǫ increases. This results decrease in Total-time. D. Plotting: Using above tabulated data, graphs were plotted for Brute-Force, CGAL and MPNN environments by varying ǫ on X-axis and Total-time on Y-axis and are as shown below. Fig. 17. ǫ. Total-time obtained for MPNN for all environments by varying Explanation: Same explanation discussed abovefig() also applied here but MPNN divides C-space in to parts and build Kd-tree in each of these parts. This results in decrease in Total-time at lower rate than CGAL. Fig. 15. Total-time obtained for Brute-force for all environments by varying ǫ. Explnation: Total-time is not varying for Brute-Force. It is almost constant at all values of Epsilon and it seems Epsilon doesn t effect the Brute-Force as shown in the Fig. This is because Brute-Force uses sorting algorithms to find nearest neighbors. while Using sorting algorithms the distance(s) from every node to all other nodes is (are) calculated for all N using Distance-metric formula. Then all the configurations are sorted out according to the distance calculated and stored in the road-map for given K as explained above). Thus, Epsilon doesn t effect the Brute-Force neighborhood finder. Observations: Following observations can be made from Fig(15), Fig(16), Fig(17) As we discussed above and from Fig(15), Epsilon doesn t effect Brute-Force and Total-time will decrease for CGAL and MPNN as we increase Epsilon. From Fig(15) for higher values of Epsilon, total-time tends to constant value for both CGAL and MPNN. From Fig(16) for CGAL, if Epsilon 0.8, decrease in total-time is too low and graph looks almost constant. From Fig(17) for MPNN, if Epsilon 1.6, decrease in total-time is too low and even negligible. E. Choosing appropriate Epsilon(ǫ) value: Hook, Maze and Clutter environments have investigated and the time is ripe to choose appropriate ǫ value, For higher values of ǫ, decrease in total-time is too low but reliability and quality of the road-map will strictly degrade as we increase ǫ. So higher value of ǫ is not preferable.

14 ǫ doesn t effect Bryte-Force at all. This is because Brute-Force uses sorting algorithms to find nearest neighbors. For CGAL, appropriate ǫ value is 0.8 because if ǫ > 0.8 decrease in total-time is too low. For MPNN, appropriate ǫ value is 1.6 because if ǫ 1.6 decrease in total-time is too low. VIII. RESULTS CGAL is the fastest Neighborhood-Finder followed by MPNN and then followed by Brute-Force. Since Brute-Force is the exact Neighborhood-Finder it is more reliable than the CGAL and MPNN. Using Brute-Force, N=10000 and K=8 will optimize Total-time taken to find nearest neighbors, reliability and quality of Road-map. Epsilon will not effect Brute-Force at all. Using CGAL, N=10000, K=8 and Epsilon=0.8 will optimize Total-time taken to find nearest neighbors, reliability and quality of Road-map Using MPNN, N=10000, K=8 and Epsilon=1.6 will optimize Total-time taken to find nearest neighbors, reliability and quality of Road-map. [10] An optimal algorithm for approximate nearest neighbor searching fixed dimensions, S. Arya, D. M. Mount, N. S. Netanyahu, R. Silverman, and A. Y. Wu, J. ACM, vol. 45, no. 6, pp , [11] Computational Geometry, Algorithms and Applications, M. de Berg, M. van Kreveld, M. Overmars, and O.Schwarzkopf, ch 5.2 and ch 5.3, [12] Planning Algorithms, Steven M. Lavalle, ch 5, pages , [13] intersection by approximate nearest neighbor search, Malcolm Slaney Michael A. Casey. Song. ISMIR 2006, pages [14] Robot motion planning: A distributed representation approach, Barraquand and J.C.Latombe, International Journal of Robotics Research, 10(6):628649, [15] Jadbabie, A., Lin, J., and Morse, A.S., 2003, Coordination of groups of mobile autonomous agents using nearest neighbor rule, IEEE Transactions on Automatic Control 49, IX. CONCLUSIONS Nearest neighborhood search is one of the fundamental problems in Motion-planning which in turn is one of the mature research fields in Robotics. Usually many researchers assume K, N and Epsilon using some heuristic techniques. This paper, after serious investigation of various environments using Brute-Force, CGAL and MPNN nearest neighborhood finders, came up with suitable values for K, N and Epsilon in order to optimize total time taken to find nearest neighbors in a given C-space with out loosing reliability and quality of the Road-map and showed the results. REFERENCES [1] Motion-Planning Crash Course, [online]. Available: [2] Improving Motion-Planning Algorithms by Efficient Nearest-Neighbor Searching, Anna Yershova,and Steven M. Lavalle, May 2006, In proc. IEEE International Conference on Robotics and Automation. [3] Choosing Good Distance Metrics and Local Planners for Probabilistic Road map methods, Nancy M. Amato, O. Burchan Bayazit, Lucia K. Dale, Christopher Jones, Daniel Vallejo, in Proc. of ICRA98 [4] Computational Geometry: Algorithms and applications, M. de Berg, M. van Kreveld, M.Overmars, O. Schwarzkopf, second edition, [5] MSL: Motion Strategy Library, S. M. LaValle, [Online]. Available: [6] MPNN: Nearest Neighbor Library for MotionPlanning, A. Yershova, 2005 [Online].Available: yershova/mpnn/mpnn.htm [7] Planning algorithms, 2004 [Online]. Available: [8] A randomized roadmap method for path and manipulation planning, N. M. Amato and Y.Wu in Proc. IEEE Int. Conf. Robot. Autom., 1996,pp [9] Probabilistic roadmaps for path planning in high-dimensional configuration spaces, L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, IEEE Trans. Robot. Autom., vol. 12, no. 4, pp , Jun.1996.