Motion planning can be defined as the task of finding a continuous path between a start configuration and a goal configuration while avoiding any collision with obstacles present in the environment. The robot and obstacle geometry are expressed in 2D or 3D workspace while the path is represented in robot’s configuration space (C-space). Robotic motion planning requires exploration of C-space. As the dimensionality of C-space increases, complete exploration becomes computationally intractable.
For higher dimensional C-space, sampling-based planning algorithms are considered state-of-the-art. It constructs a connectivity graph that implicitly represents the structure of C-space, thus minimizing exploration. The main computations involved in path planning are generating C-space samples, joining nearby collision-free samples and evaluating the collision state (in or in
) of a configuration or a local path by a collision detection module. The latter step, performing a collision check, provides exact information about the configuration but comes at the cost of increased computational load making even the sampling based techniques intractable sometimes. One way to solve the problem is to use C-space belief as a heuristic to guide the search for feasible paths. C-space belief which refers to the belief of unevaluated configurations to be collision-free is evaluated with the help of a probabilistic model of the robot’s world.
A number of different nearest neighbour methods and distance metrics have been used by researchers so far to generate the belief model. We have performed a detailed comparative analysis of these in the same validation environment to create a benchmark. To the best of our knowledge, however, all the traditional kNN methods work in the higher dimensional C-space and do not utilize any embedded topological information. We have projected the C-space to a lower dimensional space and proposed a topological method in this projected space to generate the belief model. In the end, we have also presented a comparative analysis of our topological method and the traditional nearest neighbour ones present in the literature.
2 Related Work
Path planning algorithms have widely used graph-based and sampling-based techniques. Graph-based planning algorithms like Dijkstra’s algorithm dijkstra1959note , A* hart1968formal and D*-Lite koenig2002d are used mostly in low dimensional C-spaces where explicit representation is computationally feasible. On the other hand, sampling based techniques are generally applied in high dimensional C-spaces where explicit representation becomes infeasible. Probabilistic Roadmap kavraki1994probabilistic (PRM) planner constructs a graph or a roadmap of sampled configurations and tries to find a path from start to goal state. Rapidly-exploring Random Tree lavalle1998rapidly (RRT) planner expands a tree from start state and terminates after reaching the goal state.
The original probabilistic roadmap approach explicitly evaluated all nodes and edges of the roadmap for feasibility of path planning. As a result of the expensive nature of collision-checking, the idea of reducing the number of collision checks by generating a probabilistic model of the configuration space and estimating the collision probability of configurations and paths became popular. Recent works have been done on creating a belief model of the C-space and using the probability of collision as a heuristic to guide the search over paths to obtain a feasible pathnielsen2000two . Burns et al. incrementally constructs and refines an approximate statistical model of the entire configuration space burns2005sampling . The model is used to estimate the feasibility of an edge in the roadmap reducing the need to invoke collision checker. Pan et al. pan2013faster improves the performance of sample-based motion planners by learning from prior instances. They perform instance-based learning on the approximate C-space and compute collision probability. Choudhary et al. choudhury2016pareto builds up an incremental model of the configuration space using data from previous collision tests and uses this model to search for successively shorter paths that are likely to be feasible.
For robots with large number of DOFs, planning in configuration space can often be computationally expensive. To speed up the computation, one approach is to split the planning problem into two lower-dimensional sub-problems - planning for the shoulder and elbow joints and that for the wrist joints gochev2014motion . Researchers have often projected the configuration space to a lower dimensional space and used topological methods for path planning. Pokorny et al. pokorny2016high shows that under some constraints the topological properties are preserved upon projection.
3 Problem Definition
We formulate an inexpensive probabilistic model that takes in a configuration and outputs its belief of being collision-free. The belief depends on the model and its parameters . It is a mapping from robot’s configuration space to a set . A positive value predicts the query configuration to be in while a negative value estimates it to be in , higher the magnitude higher is the corresponding probability. Mathematically, this mapping can be expressed as .
The performance of the model is evaluated using two metrics: accuracy and average error. Accuracy refers to the fraction of number of query configurations for whom the model correctly predicts its collision state. Average error refers to the mean L2 deviation between the actual collision state and the predicted collision probability of all the query configurations.
The robotic platform used in our work is HERB (Home Exploring Robot Butler) at Personal Robotics Lab, Carnegie Mellon University. HERB is a bimanual mobile manipulator comprising of two 7 DOF Barret WAM arms on a Segway base equipped with a suite of image and range sensors. The C-space mentioned in this project is the 7-dimensional joint space of HERB’s right arm. We assume that the C-space is a continuous ambient space whereby neighbouring points tend to share the same collision state, i.e., either both or neither of them are in .
4.1 Sobol Sequences
We created 8 environments typical to what HERB will find itself in everyday life like a kitchen, a room with shelf and table. 3 of these are shown in Figure 1. In each of these environments, we generated a training set of (model parameter) samples using Sobol sequences. The motivation behind preferring Sobol sequences over randomly generated ones is that the former are quasi-random low discrepancy sequences, hence, cover the space more uniformly and leave less number of voids. A Sobol sequence S
where, q is the configuration in C-space, L and U are the vectors of minimum and maximum values of the arm joint angles respectively. Operator carries out element wise multiplication of the two matrices.
Each sample is a unique configuration of the robot’s arm in its C-space which can belong to any one of the three categories:
self-collision - a link of the arm is in collision with another
- arm is in collision with some object in the environment
- not in any collision at all
Obviously, it can happen that a sample is both in self-collision and in . In those cases, we consider the sample to be in self-collision.
Collision checker module is called on the sample to determine its collision state. If it in self-collision it is discarded, otherwise included in the training set. We maintain an equal number of samples and samples in the training set. In this way, we generate a training set of samples comprising of and samples. Moreover, for each of the samples, a collision report is obtained from the collision checker module which contains information regarding the 3D position and orientation of all the in-collision points of arm. Pseudocode for generating training set having samples is shown below:
The belief of a query configuration q’ to be collision-free is evaluated as the weighted average of the collision state (-1 for and 1 for sample) of some of its neighbouring configurations from the training set.
where, is the set of neighbouring configurations of query , and are the weight and true collision state of neighbour respectively. Here, there are two parameters in estimating belief are set , and weights .
The neighbouring configurations are found out using various methods as discussed in the subsequent subsections. The weight of a neighbouring configuration q is a measure of its importance in determining the collision probability of q’. Closer is q from q’, higher is the probability of q’ sharing the same collision state as q as per the assumption of continuous ambient space, hence higher is the weight of q.
4.2 Exact Nearest Neighbours
The first method used to find the neighbouring configurations is the exact nearest neighbours or simply the nearest neighbour (NN) method altman1992introduction . nearest neighbours of the query configuration collectively evaluate its collision probability or the belief of being collision-free. The weight of configuration is taken to be the reciprocal of the euclidean distance between and , i.e,
This weight metric is consistent with the assumption of continuous ambient space. Larger the value of , closer are q’ and q, hence higher should be the weight. Figure 3 shows the performance of NN method as a function of for a fixed value of (size of training set). We also varied keeping constant and the corresponding results are shown in Figure 3.
4.3 Approximate Nearest Neighbours
Although NN method gives the closest training samples to the query but is computationally expensive both from the time and memory consumption point of view. In such circumstances, it may be a good idea to settle for a ’good guess’ of nearest neighbours in the exchange of reduced computational load. Approximate Nearest Neighbours chakrabarti2004optimal (ANN) method fulfills the requirements. Although, it doesn’t guarantee to return the closest neighbours but is computationally faster and consumes less memory than the NN method. After determining the required number of neighbours, their weights are computed in the exact same manner as in the case of NN method. Figure 4 shows the performance of ANN method as a function of for a fixed . Also, the results of varying keeping constant are shown in Figure 4.
4.4 Fixed Radius Near Neighbours
Both the NN and ANN methods find a pre-determined number of neighbours of a query configuration q’. However, it may be useful to relax this constraint on the quantity of neighbours and instead determine all the neighbours residing inside a 7-dimensional sphere of radius centred at q’. In this case, we computed the weight of configuration q using either of the two kernels:
Gaussian kernel: It is a second-order non-parametric kernel. However, it is not compact as its support runs over the entire space.
is the variance of the training set.
Epanechnikov kernel: It is a smooth non-parametric kernel. Among all the kernels it minimizes asymptotic mean integrated square error and is therefore optimal.
Each of the three methods used to compute is a function of the similarity (or closeness) of neighbour q to query q’. We refer to this quantity as the similarity estimate between configurations q and q’ denoted by . Table 1 lists the three methods used to estimate .
4.5 Importance weights
All the three weight metrics discussed so far- reciprocal of euclidean distance, Gaussian and Epanechnikov kernels- compute giving equal importance to all the dimensions of the C-space. Physically, it means that all the joints of the arm are assigned equal weight in estimating the C-space belief. In other words, what matters is the absolute difference between q’ and q and not the individual difference in each joint values. But, intuitively not all the joints should be equally important. For any manipulator, the base joint have more influence on a configuration than the wrist joints as a small perturbation in the base joint results in the motion of all the links while that in wrist joint (closer to the end-effector) affect only the wrist ones. This whole idea is illustrated in case of a planar 3R manipulator in Figure 7.
Let, be the query configuration whose belief of being collision-free is to be estimated. We take two neighbouring configurations and . Clearly, the weight of the two configurations and are equal as . However, as we can observe from the figure itself, is a closer neighbour of than is , hence should have a comparatively higher weight than in estimating the collision probability of . This ambiguity can be resolved by assigning importance weights to each joint of the robot’s arm and then computing weighted L2 norm rather than the simple L2 norm. We will refer to these weights as ’importance weight’ of joint in order to avoid any confusion with the weight of the training samples.
The importance weight of a joint is a measure of its dominance on the arm configuration. A joint which when actuated by a small amount results in a large deviation from the current configuration of the arm will have higher importance weight than others. Intuitively, shoulder joints should have higher weights than wrist joints. From the very definition, it is clear that the importance weights depend on the configuration itself. So, rather than allotting a constant weight to each joint, we assigned weight to each joint of a configuration depending on its ability to disturb the original state of the arm. Moreover, the interpretation and hence the calculation of importance weights of a sample is different from that of a sample as explained below. We denote the vector of the importance weight of configuration as . The importance weight of joint of is given by the element of .
For samples, the deviation from the original configuration is measured as the deflection in the position of the end-effector when a joint is perturbed by 0.01 radian. the initial position of end-effector be . can be computed as follows:
Increase by 0.01 radians keeping all other joint angles fixed
Determine the new position of the end-effector
Construct a vector whose element is as computed above. is given by the following formula:
In case of samples, the calculation of is a bit more involved. Since some link of the arm is in collision with some object(s) in the environment, the ability of a joint to perturb the configuration from its original state is not the same as in the previous case. Depending on the link in-collision, some joints can alter the collision state while the rest can’t. In order to calculate for sample , we need to compute two things beforehand with the help of collision report:
the link in-collision with the environment. Although there can be multiple such links, we have considered only the first link, i.e., the one closest to the shoulder of HERB. Let the index of this link be .
the centre of mass P of all the in-collision meshes of link . Lets call the vector to be
According to our convention, both joint index and link index start from 0 with link 0 being the fixed link and joint connects link and link . Note that if link of sample is in collision, then actuating any joint between link and the end-effector will not affect the link hence the collision state of the arm. In other words, whatever be the values of joint , the arm will always be in collision. This suggests that the difference in values of these joints between query and is immaterial in the computation of the weight of . Hence, the importance weight of these joints is assigned to be zero. However, the remaining joints can affect the state of configuration . The importance weight of joint () is computed in a similar manner to that in configurations. Only this time the role of end-effector is supplanted by the point P. can be computed as follows:
Increase by 0.01 radians keeping all other joint angles fixed
Determine the new position of the point P
For , . We construct a 7-dimensional vector whose element is as computed above. Finally,
For sample , we construct a 7X7 diagonal matrix whose diagonal element is . The weighted L2 norm can thus be computed as:
So far, we have mentioned two measures of estimating similarity between two configurations and - L2 norm and weighted L2 norm. This similarity estimate is then used in the calculation of weight of a training sample. In addition, we have also used scale-invariant mahalanobis distance mahalanobis1936generalized as the similarity measure. In sum, we have used four different measures for estimating similarity between configurations and as summarised below:
where is the covariance matrix of the training set.
4.6 Topological method
All the methods discussed so far to compute the C-space belief are applicable in the 7D C-space of the robot and does not capture any higher order topological structure of the environment. Exploiting the embedded structure has proved to be of significant importance in motion planning. The methods that seek to determine and exploit such structures and the topology of the space are called topological methods. They do not rely on the exact metric information and hence are robust to errors.
However, the computational complexity of such methods increase exponentially with the dimensionality of the space. Hence, practically these methods can’t be applied in a high dimensional space like the 7-D C-space. We solved the high dimensionality problem by projecting the 7-D C-space to a 4-D space referred as the ’topological space’. For any vector in the C-space, its projection in the topological space is given by the following rule:
where means the first four elements of vector . This projected space is constructed by removing the last 3 axes of the C-space. In other words, it is the state space of the first four arm joints, i.e., shoulder and elbow joints. The motivation behind this projection rule is embedded in the mechanical design of the arm itself. HERB’s arm has 2 shoulder, 2 elbow and 3 wrist joints with the wrist links being much smaller than the other links. Since the latter joints can actuate only the wrist links, they have little influence on the arm configuration as compared to the shoulder and elbow joints together. Hence neglecting them is a fairly reasonable step and does not result in much significant information loss.
One of the ways to capture the topological structure of a set of 2D points is Delaunay triangulation. Triangulation of a set of 2D points results in the formation of triangles such that no point lies inside the circumcircle of any triangle. Tessellation in n-dimensional space is a general form of triangulation in 2-D space. Tessellation forms simplices such that no point lies inside the circum-hypersphere of any simplex. Simplex is the generalization of triangle or tetrahedron to any arbitrary dimension. A 2-simplex is the triangle and 3-simplex is tetrahedron. In general, a k-simplex is a k-dimensional polytope and the convex hull of its k+1 vertices. Some pre-processing is required:
Project the entire training set to the topological space as per the projection rule
Construct the Delaunay Tessellation of the projected training set
Like in the previous methods, the belief of a configuration to be collision-free is the weighted average of the collision state of its neighbours. The steps to determine the neighbours of query are as follows:
Find the projection of query to the topological space
Determine the simplex which circumscribes .
The neighbours of the projected query configuration are the vertices of the circumscribing simplex. Obviously, it is possible that may not lie inside any simplex of the tessellation. In that case, we have assumed that the query is equally probable to be in or in and hence its belief is 0.0 (remember in our case belief varies from -1.0 to 1.0).
In this method, the weight of a neighbour of query is taken as the reciprocal of . In the case of weighted euclidean and weighted mahalanobis similarity measure, the importance weights used are also projected to the topological space using the projection rule. Moreover, the covariance matrix used in the mahalanobis and weighted mahanlanobis similarity measure is of the projected training. We call the belief of thus computed as .
In order to make up for the lose of information due to projection, we compute a secondary belief . The process of estimating is same as that of except the projection step. All the configurations and their joints importance weights are projected to a 3D space by taking their last three joint values which is ignored in the topological space. Note that the neighbours of query are estimated from the topological space only and the same neighbours are used to compute also. The net belief of configuration to be collision-free is the weighted average of and . Here, the weight of is taken to be 100 times that of .
Apart from the training size , each of the four methods - Nearest Neighbours, Approximate Nearest Neighbours, Epanechnikov Kernel and Gaussian Kernel - has a parameter which influences the performance of the model. Number of neighbors is that parameter in the first two methods while the rest have the radius of sphere . Out of all the set of parameters tried, the optimum performance is observed at for the first two methods and for the last two.
We analyzed the performance of the proposed weighted euclidean measure of computing distance metrics. The accuracy and average error of the model using the weighted euclidean measure were compared with that of the model invoking the typical euclidean measure. The results show that the weighted Euclidean measure performs better than the non-weighted one that too by a considerable margin for all the methods over the entire range of training set size. Our experiments demonstrated that it achieves higher accuracy than the latter one. In addition, the average error encountered in estimating C-space belief using this measure is less as compared to the Euclidean measure.
Moreover, we analyzed the results of using scale invariant mahalanobis distance measure over euclidean measure in computing distance metrics. The model implementing the former measure results in higher accuracy and lower average error compared to the one which invokes euclidean measure. Figure 9 shows the results of using four different measures for evaluating distance metrics in different methods.
We compared the performance of our proposed topological method with the four other methods discussed above. The results are plotted in Figure 10. As evident from the figure, the accuracy of our method is significantly higher than that of all the other baselines. In addition, the average error encountered in using topological methods is considerably lower than that in others. Figure 10 presents the comparative analysis of four measures used to evaluate the distance metrics.
6 Discussion and Future Work
In this work, we have proposed a diagonal matrix to capture the unequal significance of arm joints. Weighted L2 norm with as the weighting matrix is a more appropriate measure of the weight of a sample as compared to L2 norm. Currently, we have used the ability of a joint to move a reference point (end-effector in samples and collision center of mass in samples) as a measure of the importance weight of the sample. However, the displacement of a single point in a 7 DOF arm does not capture the manipulability of the arm in the particular configuration. We believe that a better estimate of a joint weight can be the total volume swept by the arm due to small perturbation the joint.
is a diagonal matrix because of our method of estimating joint weights. Diagonal elements are the weight of the corresponding joint in the configuration. By the same analogy, non-diagonal terms represent the coupled weight of two joints. In other words, it is like correlation between two joints. It may be an interesting idea to reason about the non-diagonal terms of the matrix and whether they can be anything non-zero.
We have used Delaunay Triangulation as the topological structure of the projected space. The neighbours of a query are taken as the vertices of the circumscribing simplex. The results of this method are better than all the other ones that are used in the C-space itself. However, when the sampling density is high, the higher order structure is lost because of the presence of large number of simplices. To overcome these shortcomings, we plan to use neighbourhood graphs like Relative Neighbourhood Graph toussaint1980relative (RNG) as the topological structure. The motivation behind doing so is the fact that RNG of a set of points closely matches human perception of the shape of the set. The neighbours of a query can be taken as the vertices of the polygon enclosing the query.
-  Edsger W Dijkstra. A note on two problems in connexion with graphs. Numerische mathematik, 1(1):269–271, 1959.
-  Peter E Hart, Nils J Nilsson, and Bertram Raphael. A formal basis for the heuristic determination of minimum cost paths. IEEE transactions on Systems Science and Cybernetics, 4(2):100–107, 1968.
-  Sven Koenig and Maxim Likhachev. D^* lite. Aaai/iaai, 15, 2002.
-  Lydia Kavraki, Petr Svestka, and Mark H Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces, volume 1994. Unknown Publisher, 1994.
-  Steven M LaValle. Rapidly-exploring random trees: A new tool for path planning. 1998.
-  Christian L Nielsen and Lydia E Kavraki. A two level fuzzy prm for manipulation planning. In Intelligent Robots and Systems, 2000.(IROS 2000). Proceedings. 2000 IEEE/RSJ International Conference on, volume 3, pages 1716–1721. IEEE, 2000.
-  Brendan Burns and Oliver Brock. Sampling-based motion planning using predictive models. In Robotics and Automation, 2005. ICRA 2005. Proceedings of the 2005 IEEE International Conference on, pages 3120–3125. IEEE, 2005.
-  Jia Pan, Sachin Chitta, and Dinesh Manocha. Faster sample-based motion planning using instance-based learning. In Algorithmic Foundations of Robotics X, pages 381–396. Springer, 2013.
-  Shushman Choudhury, Christopher M Dellin, and Siddhartha S Srinivasa. Pareto-optimal search over configuration space beliefs for anytime motion planning. In Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on, pages 3742–3749. IEEE, 2016.
-  Kalin Gochev, Venkatraman Narayanan, Benjamin Cohen, Alla Safonova, and Maxim Likhachev. Motion planning for robotic manipulators with independent wrist joints. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pages 461–468. IEEE, 2014.
-  Florian T Pokorny, Danica Kragic, Lydia E Kavraki, and Ken Goldberg. High-dimensional winding-augmented motion planning with 2d topological task projections and persistent homology. In Robotics and Automation (ICRA), 2016 IEEE International Conference on, pages 24–31. IEEE, 2016.
-  Naomi S Altman. An introduction to kernel and nearest-neighbor nonparametric regression. The American Statistician, 46(3):175–185, 1992.
-  Amit Chakrabarti and Oded Regev. An optimal randomised cell probe lower bound for approximate nearest neighbour searching. In Foundations of Computer Science, 2004. Proceedings. 45th Annual IEEE Symposium on, pages 473–482. IEEE, 2004.
-  Vassiliy A Epanechnikov. Non-parametric estimation of a multivariate probability density. Theory of Probability & Its Applications, 14(1):153–158, 1969.
-  Prasanta Chandra Mahalanobis. On the generalized distance in statistics. National Institute of Science of India, 1936.
-  Godfried T Toussaint. The relative neighbourhood graph of a finite planar set. Pattern recognition, 12(4):261–268, 1980.