efficient nearest neighbor searching for motion planning anna atramentov dept. of computer science...
TRANSCRIPT
![Page 1: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/1.jpg)
Efficient Nearest Neighbor Searching for Motion Planning
Anna Atramentov
Dept. of Computer Science
Iowa State University
Ames, IA, USA
Steven M. LaValle
Dept. of Computer Science
University of Illinois
Urbana, IL, USA
Support provided in part by an NSF CAREER award.
![Page 2: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/2.jpg)
Motivation
Statistics Pattern recognition Machine Learning
Nearest neighbor searching is a fundamental problem in many applications:
PRM-based methods RRT-based methods
In motion planning the following algorithms rely heavily on nearest neighbor algorithms:
![Page 3: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/3.jpg)
Basic Motion Planning Problem
Given: 2D or 3D world Geometric models of a robot and obstacles Configuration space Initial and goal configurations
Task: Compute a collision free path that connects initial and goal
configurations
![Page 4: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/4.jpg)
Probabilistic roadmap approaches(Kavraki, Svestka, Latombe, Overmars, 1994)
The precomputation phase consists of the following steps:
1. Generate vertices in configuration space at random
2. Connect close vertices
3. Return resulting graph
Obstacle-Based PRM (Amato, Wu, 1996); Sensor-based PRM (Yu, Gupta, 1998); Gaussian PRM (Boor, Overmars, van der Stappen, 1999); Medial axis PRMs (Wilmarth, Amato, Stiller, 1999; Psiula, Hoff, Lin, Manocha, 2000; Kavraki, Guibas, 2000); Contact space PRM (Ji, Xiao, 2000); Closed-chain PRMs (LaValle, Yakey, Kavraki, 1999; Han, Amato 2000); Lazy PRM (Bohlin, Kavraki, 2000); PRM for changing environments (Leven, Hutchinson, 2000); Visibility PRM (Simeon, Laumond, Nissoux, 2000).
The query phase:
1. Connect initial and goal to graph
2. Search the graph
![Page 5: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/5.jpg)
Rapidly-exploring random tree approachesGENERATE_RRT(xinit, K, t)1. T.init(xinit);
2. For k = 1 to K do
3. xrand RANDOM_STATE();
4. xnear NEAREST_NEIGHBOR(xrand, T);
5. u SELECT_INTPUT(xrand, xnear);
6. xnew NEW_STATE(xnear, u, t);
7. T.add_vertex(xnew);
8. T.add_edge(xnear, xnew, u);
9. Return T;
xnear
xrand
xinit
xnew
LaValle, 1998; LaValle, Kuffner, 1999, 2000; Frazzoli, Dahleh, Feron, 2000; Toussaint, Basar, Bullo, 2000; Vallejo, Jones, Amato, 2000; Strady, Laumond, 2000; Mayeux, Simeon, 2000; Karatas, Bullo, 2001; Li, Chang, 2001; Kuffner, Nishiwaki, Kagami, Inaba, Inoue, 2000, 2001; Williams, Kim, Hofbaur, How, Kennell, Loy, Ragno, Stedl, Walcott, 2001; Carpin, Pagello, 2002.
The result is a tree rooted at xinit:
![Page 6: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/6.jpg)
GoalsExisting nearest neighbor packages:
ANN (U. of Maryland) Ranger (SUNY Stony Brook)
Problem: They only work for Rn. Configuration spaces that usually arise in motion planning are products of R, S1 and projective spaces.
Theoretical results:
Problem: Difficulty of implementation
P. Indyk, R. Motwani, 1998; P. Indyk, 1998, 1999;
Our goal:Design simple and efficient algorithm for finding nearest neighbor in these topological spaces
![Page 7: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/7.jpg)
Literature on NN searching
• It is very well studied problem• Kd-tree approach is very simple and efficient
• T. Cover, P. Hart, 1967 • D. Dobkin, R. Lipton, 1976 • J. Bentley, M. Shamos, 1976 • S. Arya, D. Mount, 1993, 1994• M. Bern, 1993• T. Chan, 1997• J. Kleinberg, 1997• K. Clarkson, 1988, 1994, 1997• P. Agarwal, J. Erickson, 1998 • P. Indyk, R. Motwani, 1998 • E. Kushilevitz, R. Ostrovsky, Y. Rabani, 1998• P. Indyk, 1998, 1999• A. Borodin, R. Ostrovsky, Y. Rabani, 1999
![Page 8: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/8.jpg)
Problem FormulationGiven a d-dimensional manifold, T, represented as a polygonal schema, and a set of data points in T.Preprocess these points so that, for any query point q T, the nearest data point to q can be found quickly.
The manifolds of interest: Euclidean one-space, represented by (0,1) R. Circle, represented by [0,1], in which 0 1 by identification. P3, represented by [0, 1]3 with antipodal points identified.
Examples of 4-sided polygonal schemas:
cylinder torus projective plane
![Page 9: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/9.jpg)
Example: a torus
47
6
5
1
3
2
9
8
10
11
q
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
47
6
5
1
3
2
9
8
10
11
![Page 10: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/10.jpg)
Algorithm presentation
Overview of the kd-tree algorithm
Modification of kd-tree algorithm to handle topology
Analysis of the algorithm
Experimental results
![Page 11: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/11.jpg)
Kd-trees
The kd-tree is a powerful data structure that is based on recursively subdividing a set of points with alternating axis-aligned hyperplanes.
The classical kd-tree uses O(dn lgn) precomputation time, O(dn) space and answers queries in time logarithmic in n, but exponential in d.
47
6
5
1
3
2
9
8
10
11
l5
l1 l9
l6
l3
l10 l7
l4
l8
l2
l1
l8
1
l2 l3
l4 l5 l7 l6
l9l10
3
2 5 4 11
9 10
8
6 7
![Page 12: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/12.jpg)
Kd-trees. Construction
47
6
5
1
3
2
9
8
10
11
l5
l1 l9
l6
l3
l10 l7
l4
l8
l2
l1
l8
1
l2 l3
l4 l5 l7 l6
l9l10
3
2 5 4 11
9 10
8
6 7
![Page 13: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/13.jpg)
Kd-trees. Query
47
6
5
1
3
2
9
8
10
11
l5
l1 l9
l6
l3
l10 l7
l4
l8
l2
l1
l8
1
l2 l3
l4 l5 l7 l6
l9l10
3
2 5 4 11
9 10
8
6 7
q
![Page 14: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/14.jpg)
Algorithm Presentation
l1
l8
1
l2 l3
l4 l5 l7 l6
l9l10
3
2 5 4 11
9 10
8
6 7
q
47
6
5
1
3
2
9
8
10
11
l5
l1 l9
l6
l3
l10 l7
l4
l8
l2
1
3
l4
l8
l2
![Page 15: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/15.jpg)
Analysis of the Algorithm
Proposition 1. The algorithm correctly returns the nearest neighbor.
Proof idea: The points of kd-tree not visited by an algorithm will always be further from the query point then some point already visited.
Proposition 2. For n points in dimension d, the construction time is O(dn lgn), the space is O(dn), and the query time is logarithmic in n, but exponential in d.
Proof idea: This follows directly from the well-known complexity of the basic kd-tree.
![Page 16: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/16.jpg)
Experiments
For 50,000 data points 100 queries were made:
dim topology new algorithm new algorithm brute force
construction time
100 queries time
100 queries time
3 S1 x S1 x S1 5.6s 0.1s 29.2s
6 R3 x P3 7.1s 3.2s 68.9s
13 R3 x (S1)3 x (P3)2 8.8s 9.2s 153.1s
![Page 17: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/17.jpg)
ExperimentsPRM method
topology
new algorithm brute force
nodes time(s) nodes time(s)
R3 x P3 11,684 96.83 11,698 247.29
27,076 147.58 27,259 740.42
Success 42,630 173.43 42,714 1,229.70
58,117 215.16 58,308 1,796.17
![Page 18: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/18.jpg)
ExperimentsRRT method
topology
new algorithm brute force
nodes time(s) nodes time(s)
R2 x S1 37,177 584.9 39,610 7,501.4
![Page 19: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/19.jpg)
topology
new algorithm brute force
nodes time(s) nodes time(s)
(R3 x P3)8 17,271 4,631.9 18,029 8,461.6
ExperimentsRRT method
![Page 20: Efficient Nearest Neighbor Searching for Motion Planning Anna Atramentov Dept. of Computer Science Iowa State University Ames, IA, USA Steven M. LaValle](https://reader035.vdocument.in/reader035/viewer/2022062804/5697bf991a28abf838c91ddb/html5/thumbnails/20.jpg)
Conclusion
We extended kd-tree to handle topology of the configuration space
We have presented simple and efficient algorithm
We have developed software for this algorithm which will be included in Motion Strategy Library (http://msl.cs.uiuc.edu/msl/)
Future Work
Extension to more efficient kd-trees
Extension to different topological spaces
Extension to different metric spaces