Chapter 3 Planning in the State Space
3.3 Nearest-Neighbor Searching
Nearest-neighbor searching is a very important problem in many applications, such as pattern reconfiguration, statistics, and machine learning. It is also essential in the sampling-based motion planning. Probabilistic roadmap (PRM) approaches [2, 25]
build a graph of collision-free paths that attempts to capture the connectivity of the configuration space. The vertices in configuration space are generated using random sampling, and attempt to connect each vertex to nearby vertices. However, most roadmaps often contain thousands of vertices, which can lead to substantial computation time for searching nearby vertices. The Rapidly-exploring Random Trees (RRTs) [31, 32] approaches also spend a lot of computation time on the nearest neighbors search. When RRTs grows, a random configuration is chosen, and the RRTs vertex that is closest with respect to a predefined metric. The randomly-chosen state will attempt to connect the closest RRT vertex. If the planner can efficiently find nearest neighbors, it can greatly improve the performance of these path planning.
Especially planning in high dimensional space as humanoid robot arms, a good nearest neighbor search can save a lot of time. Because the arm has many degrees of freedom, it will be time-consuming to search entire space. In other words, if dimension of configuration space is higher, the algorithm which is based on KD-tree
In this section we will introduce some popular data structures and search methods. Finally, we will combine some of these approaches to create an algorithm which makes the nearest neighbor search more efficient in the high dimensional space.
3.3.1 K-Dimension tree
The KD-tree is a powerful data structure that based on splitting planes which are perpendicular to one of the coordinate system axes. Hence, the KD-tree is very efficient in the nearest neighbor search, especially in the large number of nodes. The class KD-tree takes Ο
(
dnlogn)
pre-computation time, and querying a nearby vertex in a balanced KD-tree needs Ο(
n1 1 d−)
time. There are some approaches to improve the efficiency of data structure and its construction algorithm in the context of nearest neighbor search [38, 41, 46].The ANN (Approximate Nearest Neighbor Library) approach is a very efficient data structure, which is not only based on kd-tree and but also enhances the efficiency of KD-tree. Many researches have cited its approach and even some researches are based on it. In this section, We introduce dividing rules according to ANN programming manual [41], which divides the current cell through its midpoint orthogonal to its longest side. If there are ties, it chooses the dimension with longest
point spread. Figure 3-6 illustrates how the splitting rule is executed and how the corresponding binary tree looks for the data points on a torus [3].
4
Figure 3-6 KD-tree construction [3]
In order to perform the nearest neighbor calculation, the tree is searched in a depth-first fashion and at each stage it makes an approximation to the nearest distance.
When the algorithm decides that there cannot possibly be a closer point it terminates, giving the nearest neighbor. For example, we give a target point which is not in the tree, then beginning to search nearest neighbor. First, the KD-tree compares the target with root of tree. If target is smaller than root, we take target to compare next point of the left part of tree, and vice versa. Iterating this step until the nearest neighbor is found. The Figure 3-7 simply illustrates the query steps.
l1
3.3.2 MPNN
The MPNN (Nearest Neighbor Library for Motion Planning) is very powerful library for motion planning, which can adapt many scenarios of planning. In other word, MPNN can be applied to many kinds of the topology of configuration space.
And it even makes the nearest neighbor algorithm dynamic, which utilize the point insertion operation with tree rebalancing.
Generally, the configuration space of motion planning does not only consist of Euclidean space. And it often composes of other metric space, such as the metric space of circle. For example, 2D rigid body freely translating and rotating in the plane has the configuration spaceC= \2 1S , in which circleS represents the 2D rotations. 1 3D rigid body rotations lead to three-dimensional real projective space
configurations\P3. The metric information should be appropriately processed for the data structures of search, because the data must be generated by correct nearest neighbor computation. According to [3], the approach defines some common metric space in the MPNN library, which includes Euclidean one-space \ , circles S , real 1 projective space \P3, and Cartesian products of the space. Table 3-3 shows the algorithm for building KD-tree in the topology space. As for further detail of manifolds of topology space, please see [57].
According to [57], MPNN is not only based on ANN library but also enhance some capability. However the ANN library is static KD-tree, that is to say, ANN library can not add any point when the tree of data structure was already built. It is costly to ensure that the trees are balanced. For n points, there is a tree that contains 2i points for each “l” in the i place of the binary representation of n. When bits th are cleared in the representation due to increasing n, the trees are deleted, and the point are included in a tree that corresponds to the higher-order bit which changed to
“l” [57]. In accordance of [57] this general scheme incurs logarithmic-time overhead, regardless of dimension.
Table 3-3 The pseudo-code for constructing KD-tree in Topology space [57].
BUILD KD TREE(P, d, T, m, b, s)
Input: A set of points, P, the dimension of the space, d, the topology of the space, T, the number of points to store in a leaf, m, the bounding box, b, for P, and the
splitting rule, s.
Output: The root of a KD-tree storing P 1. if P contains less than m points
2. then return a Leaf storing these points
3. else split b into two subboxes, b1, b2, according to s by plane l, orthogonal to dimension k.
4. Find P1 and P2, the sets of the data points falling into boxes b1 and b2. 5. v1 =BUILD KD TREE(P1; d; T; m; b1; s)
6. v2 =BUILD KD TREE(P2; d; T; m; b2; s)
7. Create a Node v storing the splitting plane, l, the splitting dimension, k, the topology of the space TK of this dimension, the projection of the box, b, on TK, and v1 and v2, the children of v.
8. return v
3.3.3 Hybrid Nearest-Neighbor Search
By these introductions in above-mentioned sections, we know some good methods of nearest neighbor search. But the motion planning for arm is in the high-dimension configuration space. We do not know whether these approaches are as efficient as the planning which in the 2D or 3D configuration space. In addition, we plan path often in the simple environment, except in the some extreme case. That is to say, the tree of data structure doesn’t have too many nodes, so it doesn’t cost too much time to execute nearest neighbor search. Therefore we do some simulation and experiment to observe the performance of the class KD-tree and MPNN. From the Figure 3-8 (b) we can find efficiency of KD-tree and MPNN are not good in the high dimension configuration space when number of nodes is few. Even the brute force search is better than them. So we need some approaches spend the least computation time no matter the number of nodes is many or few.
In our case, we do not need planning in the Topology of configuration space, but only in the Euclidean space. We can find the brute force search has good performance when number of nodes is fewer than eight hundreds form the Figure 3-8(b), and the brute force does not need building time. So we want to construct one method called HNN (Hybrid Nearest Neighbor), which is using the brute force to do querying when number of nodes is few. And we can also use brute force to balance ANN kd-tee
rebuilding times when the number of nodes is more than the threshold.
0 1000 2000 3000 4000 5000 6000 node
ms KD-Tree
0 1000 2000 3000 4000 5000 6000 node
ms KD-Tree
(Class) MPNN Brute Force
Figure 3-8 Nearest neighbor algorithms of computation time comparison
From the 3.3.1 section, we can know the best data structure of the KD-tree is constructed by 2n number of nodes (n is a nature number)[41]. So, when number of nodes is over 2n (n = 1, 2, 3, 4…), building or rebuilding ANN KD-tree. But arriving this step has one problem, which is using one tree to iterate or using multi-trees like MPNN algorithm[57]. Therefore we are using these two different methods to experiment and compare them. Form the Figure 3-9, we can find that the method which combines ANN with brute force is faster than class KD-tree and MPNN in the querying, no matter it is using one-tree to iterate or multi-trees.
Although the HNN for one-tree is the fastest in the querying, it has longer time in the building data structure. How to balance between the building and the querying to make the motion planning for arm optima? In next section, we will compare these
(a) Building data structure (b) Nearest neighbor search
methods in our scenario and find the most suitable method.
Figure 3-9 Nearest neighbor algorithms of computation time comparison
3.3.4 Summary
The nearest neighbor algorithms are not suitable for all case, but they often have better performance in case by case. So we want to choose the best nearest neighbor search for our case.
0 1000 2000 3000 4000 5000 6000 node
ms KD-Tree
0 1000 2000 3000 4000 5000 6000 node
ms KD-Tree
In our case, the motion planning is realized by the algorithm which is based on RRT, and its distance metric is in the Euclidean spaces. Therefore, the planner is adding node to build the tree and at the same time querying the nearest neighbor node.
In Figure 3-10, this scenario is assume that no any obstacle in the environment, and we can know that the HNN for one-tree has the best performance in our simulation environment. Furthermore, the HNN for the multi-trees is also faster than the class KD-tree and MPNN. Therefore, we can say that the HNN is the best choice in our case (in the high dimension and Euclidean space).
Sim ulation planning
0 1000 2000 3000 4000 5000 6000 Node
ms KD-Tree
Figure 3-10 Scenario of motion planning by RRTs