Motion planning for robotics is the task of finding a sequence of feasible robot states between a start and goal state. Feasible robot states are those that satisfy problem-specific constraints, generally one of which is to remain out of collision from objects in the environment. Motion planning for a robot is often performed in its configuration space (C-space), a space in which each element represents a unique configuration of the robot. Each configuration in the robot’s C-space can belong to one of two subspaces: or . A configuration is in the collision-free subspace if the robot is not in contact with any workspace obstacle when in that corresponding configuration; otherwise, the configuration is in the in-collision subspace [1, 13]. For robot manipulators with many controllable degrees-of-freedom (DOF), the dimensionality of the C-space may be large as each joint contributes to defining the robot’s configuration. An example workspace and its corresponding C-space for a 3 DOF robot arm are shown in Fig. (a)a-(b)b.
Determining feasibility of a robot state usually involves discrete collision checking to determine whether the query robot state would intersect with an environmental obstacle . Repeated collision checking is computationally expensive and takes up to 90% of the computation time during motion planning . An open challenge exists for real-time, accurate, dynamic robot planning in changing environments; to meet this need, we seek to create an accurate C-space model that may be used as a faster proxy to the more computationally-burdensome geometric collision checkers upon which most motion planners currently rely.
In this paper, we present a new kernel function, or similarity function, for improved configuration space modeling for robot manipulators. We show improved proxy collision checking performance in terms of both collision check times and accuracy with the new kernel, and we demonstrate the kernel’s usefulness in accelerating motion planning.
In previous work, we described the Fastron algorithm, which creates a nonparametric model inspired by the kernel perceptron algorithm and efficiently checks for changes in the C-space due to moving workspace obstacles. The input into the Fastron model is a robot configuration, and the output is its predicted collision status. In previous work, a radial basis function (RBF) was used for the kernel function, which isotropically compares the joint values of a robot manipulator. While the RBF kernel is an adequate general-purpose kernel function, it fails to capture the influence each joint has in the manipulator’s workspace. In this work, we replace the RBF kernel function with a new kernel function we call the forward kinematics (FK) kernel. The benefits of replacing the RBF kernel with the FK kernel may be observed in Fig. (c)c-(d)d, where the Fastron model closely resembles the ground truth with the FK kernel but is uneven and requires many more points to represent the model when using the original RBF kernel.
We show that the FK kernel is positive definite and prove our modeling algorithm can always find a set of weights that correctly predicts the collision status of a training set. We also show that when using the Fastron model as a proxy collision checker, using the FK kernel can provide collision status predictions twice as fast as when using the RBF kernel and requires approximately an order of magnitude fewer support points to represent the model. Using the FK kernel improves the accuracy of collision status prediction to be over 95% for a 7 DOF robot arm, a significant improvement over the 75% accuracy achieved with the RBF kernel. We apply the Fastron algorithm with the FK kernel to various motion planners, demonstrating up to 3 times faster motion plans compared to when using other collision checkers.
I-B Related Work
The following works describe proxy checking methods that try to avoid computationally-burdensome collision checking and methods that utilize control points (which are used in the proposed FK kernel) for other kinematic contexts.
I-B1 Configuration Space Modeling
, Gaussian mixture models (GMMs), k-nearest neighbor models (KNNs) , and kernel perceptrons . Most of these methods provide either high accuracy but very poor computational speed, or high computational speed but poor accuracy and limited capacity to model changing environments.
Pan et al.’s SVM method learns a C-space model for a pair of rigid-body objects 
. Their method provides near-perfect models because they employ an active learning strategy that iteratively improves the precision of the model, and they use their SVM-based model for distance/penetration depth estimation between two bodies using a constrained nonlinear optimizer. However, to apply this method to a kinematic chain, a new model must be learned offline for each pair of objects, including each link in the kinematic chain and each workspace obstacle.
In , active learning of a GMM model is associated with sample-based motion planners itself by using the samples from a rapidly-exploring random tree (RRT) motion planner to modify the GMM. GMMs are constrained by the fixed dimensionality of the model.
KNNs have also been used for faster proxy collision detection for static environments . Locality-sensitive hashing is utilized for extremely fast query times. However, the problem of rehashing given changing environments makes this unsuitable for changing environments.
Our Fastron algorithm uses a kernel perceptron model to globally represent the C-space and uses an active learning strategy to adapt to a changing workspace. It was shown that collision checks may be an order of magnitude faster than state-of-the-art collision detectors . However, the accuracy of the model must be improved to reduce the search time for valid configurations and to reduce the amount of time spent repairing infeasible parts of a motion plan.
While each of these learning-based methods generates an approximate C-space model, the workspace and C-space are treated as completely separate entities and the link that relates the two spaces is more or less neglected. In this work, we aim to link the two spaces via FK.
I-B2 Control Points for Collision-Free Motion
Control points are a set of hand-selected points placed on a robot and their locations may be used to indicate the position of a rigid-body robot or a kinematic chain manipulator. Control points should be chosen to constrain the configuration of the robot, and the minimum number of these points depends on the number of DOF of the robot . FK provides the positions of these control points for manipulators given a set of joint values, and we consider FK to be the link that relates a robot’s position in C-space to its position in the workspace.
Artificial potential fields can be used for motion planning in a robot’s C-space, where the sum of attractive and repulsive forces drive the robot away from C-space obstacles and toward a goal . Potential fields can be applied to kinematic chain manipulators using control points placed along the kinematic chain .
RelaxedIK, a motion-synthesis method for manipulators, uses a neural network to estimate the risk of self-collision for an input robot configuration. Control points are placed at each joint and their locations in the workspace are included in the input to the network. The use of control points allows estimation of the self-collision costs two orders of magnitude faster than a distance-based method, and optimizing over these self-collision costs allows finding configurations that are free from self-collisions.
In this work, we utilize control points in creating a similarity function between manipulator configurations, and we leverage how FK relates a manipulator’s C-space to its workspace for more accurate proxy collision checking.
Ii-a Forward Kinematics
FK for a kinematic chain determines the pose (position and orientation) of a point on the chain given the joint values. When using the standard Denavit-Hartenberg (D-H) convention, the homogenous transformation matrix of the pose of joint in the frame of joint is
where and are the distances between the joints’ origins along the -axis and -axis, respectively, and are the angles between the joints’ -axes measured along the -axis and vice versa, respectively, and and represent and , respectively. and are the joint variables for revolute and prismatic joints, respectively, and a configuration of a robot with DOF is represented as a -dimensional vector of these joint variables. The other D-H parameters are fixed depending on the geometry of the robot. Note that when using the standard D-H convention, the origin of the frame of the joint is placed at the distal end of the link. For more details, see .
Starting with the base transform relative to a world frame , the pose of the joint is , where each transform in the chain is dependent on the robot configuration as described in the previous paragraph. The pose for an arbitrary point on the link is found by appending an additional static transformation to the pose of the joint: .
Let extract the position from a homogeneous transformation matrix, i.e., the first three elements of the last column. We can thus represent the position of the control point for configuration as
where is the static transform applied to the the pose of the joint with index .
Ii-B Binary Classification and Kernel Functions
A binary classifier predicts the class label of a query point. A binary classifier model is fit to a datasetand its associated vector of labels . Each represents a configuration of a robot. We let denote and denote .
Binary classification models often use kernels to describe nonlinear seperation of classes. A kernel function is a similarity function that should provide a large score for similar configurations and a low score for dissimilar configurations. RBF kernels, which are functions of the distance between two points, such as the Gaussian kernel, are popular. In this paper, we use the second-order rational quadratic kernel  for all RBF kernels, defined as
where is a parameter dictating the width of the kernel.
Each element of a kernel matrix is defined elementwise for each pair of points in : . is called positive definite (PD) if , and equality occurs only for . is considered to be positive semidefinite (PSD) if equality may occur for .
A PD kernel is one that yields a PD kernel matrix when each configuration in set is unique . Positive definiteness is an important property for a kernel function because it guarantees nonsingularity for the kernel matrix and represents an implicit mapping to a higher dimensional feature space according to Mercer’s theorem . PSD kernels and matrices do not have the same guarantees their PD counterparts have. is an example of a PD kernel . Summing PD kernels or multiplying PD kernels with a scalar also yields a PD kernel .
Iii-a Forward Kinematics Kernel
The major insight of this paper is in a more intuitive distance metric by computing distances in the workspace rather than in the joint space. Computing distances directly between joint configurations does not take into consideration the robot’s position in the workspace. Consider the blue two-link arm shown in Fig. (a)a. Comparing the pink and green configurations to the blue, the pink configuration appears much closer to the blue than the green does. However, the distance in the C-space between the blue and pink configurations (represented as colored dots) is equal to that of the blue and green configurations as can be seen in Fig. (b)b and (c)c. Consequently, according to the RBF kernel, the blue configuration is as similar to the pink configuration as it is the green configuration as shown in Fig. (b)b.
Rather than comparing the joint values of two configurations directly, we use FK and compare distances in the workspace between control points on the arm. We can thus define the FK kernel as a sum of RBF kernels evaluated between each corresponding control point location:
where provides the position of the control point and is the number of control points representing the kinematic chain.
In this paper, we only place control points at joint frame origins (using the standard D-H convention) by setting for each control point. In other words, a control point is placed at the distal end of each link. However, control points may be placed at arbitrary locations along the manipulator if desired. If the origins of the frames of two subsequent joints in the kinematic chain are coincident, we only place one control point at the overlapping origin. In other words, using the standard D-H convention, if a revolute joint has both and equal to 0, a control point is not placed for this joint as its origin overlaps with the preceding joint origin.
Fig. (c)c provides a visualization of the FK kernel for the blue two-link arm in Fig. (a)a, where control points are placed at the location labeled “Elbow Joint” and “End Effector”. According to the FK kernel, the similarity score between the blue and pink configurations is about 0.8, while the similarity between the blue and green is about 0.1. The similarity scores produced by the FK kernel better represent the differences we perceive in the positions of the robots in the workspace.
Iii-A2 Positive Definiteness
As previously mentioned, a PD kernel guarantees a PD and nonsingular kernel matrix and represents an implicit mapping to a feature space. The RBF kernel defined in Eq. 3 evaluated on dataset will give rise to a PD kernel matrix as long as each configuration in is unique. We show below this is also true for the FK kernel.
The FK kernel matrix based on the FK kernel defined in Eq. 4 is a sum of RBF kernel matrices evaluated on control points: . We henceforth refer to the RBF kernel matrices that comprise the FK kernel matrix as summand kernel matrices. The summand kernel matrix is PD as long as the locations of the control point (determined by applying to the configurations in ) are unique. However, depending on the choices of control points or kinematic redundancy, it is possible for two unique configurations in to yield the same control point location. For example, Fig. 3 shows four unique configurations of a two-link arm, but there are only three unique locations for each control point (represented as a hollow square and circle on each configuration). With coincident control point locations, the summand kernel matrices are demoted to PSD.
Despite possibly having PSD summand kernels, the FK kernel matrix is PD if the intersection of the nullspaces of the summand matrices is the zero vector.
The FK kernel matrix is PD if the nullspaces of each summand kernel matrix share only the zero vector, .
Assume . Thus, .
For to be PD, it must also be nonsingular, which means . Thus, . ∎
What Claim 1 means intuitively is that even if some control point locations coincide for two unique configurations, the FK kernel is PD if these configurations are distinguishable using the locations of the other control points. For example, in Fig. 3, the locations of the control points marked with hollow circles differentiate the configurations with coincident control points marked with squares, and vice versa. Note that in practice, if the configurations in are generated via random sampling, control point locations should almost surely be unique.
Iii-B Fastron Model and Algorithm
The Fastron algorithm (described in detail in ) generates a binary classification model that is used for proxy collision checking. The Fastron model is a weighted sum of kernel functions: , where is a vector of model weights, is a set of configurations, and is a PD kernel. Proxy collision detection is performed by determining the sign of , where denotes the query configuration is predicted to be in and denotes . is considered to be correctly classified if its margin is positive, where is its true label.
The weight vector is found by minimizing the loss
subject to , where is a vector of labels for dataset , is the kernel matrix for , and is a diagonal matrix where the diagonal element is and is a user-selected conditional bias parameter, which biases the model toward predicting
for more conservative predictions. This loss function is similar to that used in SVMs. The Fastron algorithm utilizes greedy coordinate descent and exits as soon as the margin , where . Some advantages of greedy coordinate descent we realize are it promotes model sparsity (and thus computational efficiency), allows us to perform lazy kernel matrix evaluation (i.e., a column of can be computed and stored only when needed), and allows us to define a cheap update rule (described below) .
In what follows, parenthetical superscripts denote the training iteration upon which the given value depends. On iteration , and are determined incrementally:
where is the standard basis vector. The descent direction, as stated previously, is determined greedily by selecting the point with the most negative margin:
This descent rule guarantees positive margin for all points in in a finite number of iterations.
If , an upper bound on the change in loss per descent step is . A lower bound of is for nonsingular . A loose upper bound on the number of descent steps required to reach from initial loss is thus . The margin is for when , which is positive because .
If , the model at iteration successfully provides a positive margin for all samples. ∎
Claim 2 means the weight update algorithm can terminate once all training samples have positive margin or will otherwise work toward achieving positive margin for all samples. Claim 2 is contigent on being nonsingular, which is shown to be the case for the FK kernel in Claim 1.
After finding a model with positive margin for all training points, points in the training set with nonzero value in comprise the support set . To promote model sparsity, support points are removed from (by setting their corresponding weight to 0) if they would have positive margin even after their removal from .
The Fastron model update algorithm is summarized in Algorithm 1. An iteration limit and a limit on the size of the support set can be set to limit update times and model sizes at the cost of model accuracy. initializes the values in each data structure to 0 or loads a previously-trained model if it exists, computes the column of , and removes elements from any data structure corresponding to a non-support point because these points are not needed for classification.
After model updating, a two-stage active learning strategy searches for changes in the C-space in case workspace obstacles move. In the first stage, random samples are generated near each support point to search for small perturbations to the C-space obstacles. In the second stage, uniformly random samples are generated in the C-space to search for new obstacles entering the workspace of the robot. A geometric collision check is performed for each support point and new sample generated via active learning to update . With the updated training set, the model update procedure is repeated.
In this section, we evaluate the performance of the FK kernel by examining how it improves proxy collision checking and motion planning. We use the 7 DOF arms of the Baxter robot for these experiments. We use Fastron with the proposed FK kernel and with the original RBF kernel, denoted as Fastron FK and Fastron RBF, respectively. We select the parameters for Fastron FK and Fastron RBF with a grid search. The geometric collision detectors compared are GJK  and FCL . GJK can determine whether two convex shapes are intersecting, and FCL is the collision detection library used in the MoveIt! motion planning framework . We use GJK as a faster, approximate geometric method and use FCL as the ground truth collision method.
Iv-a Collision Checking
|Method||Query Time||Update Time|
To compare collision checking performance, we compare model correctness, query times, model sizes, and model update times. Measures of model correctness include the overall classification accuracy, true positive rate (TPR), and true negative rate (TNR), defined as
where and represent the number of samples in the test set that are correctly and incorrectly predicted to be in , respectively, and and represent the same but for the class. TPR and TNR are useful measures of correctness when the volumes of and are unbalanced. Model size is the count of support points required to represent the model for the proxy collision check methods. The results of the experiments are averaged over 50 trials, where each trial uses one randomly-sized obstacle that moves up to 30 times in a random direction, and the models are updated between each obstacle movement.
The query timings, model sizes, and model update timings are tabulated in Table I. The initial model training (i.e., labeling 5000 random configurations using FCL and training the model starting from ) took on average 134.0 ms and 164.8 ms for Fastron FK and Fastron RBF, respectively. The majority of this initial training time (approximately 115 ms) for both methods is dedicated toward labeling the dataset using FCL. While Fastron FK has 7.8 times fewer support points than Fastron RBF, the FK kernel is more expensive to compute than the RBF kernel, causing Fastron FK to be only 1.8 times faster than Fastron RBF for query times. Fastron FK beats GJK and FCL by a factor of 5.8 and 9.2, respectively, for query times. Since there are fewer support points in the Fastron FK model, the update time (largely throttled by labeling new data via FCL) is about 2.3 times faster for Fastron FK than for Fastron RBF.
The correctness of the approximate collision methods are shown in Fig. 4, illustrating the advantages of Fastron FK over Fastron RBF. The accuracy, TPR, and TNR of Fastron FK are much higher than those of Fastron RBF. In fact, Fastron FK performs similarly in terms of correctness to GJK, but is 5.8 times faster. Fastron FK achieves an accuracy of 96.4%, a significant improvement over Fastron RBF’s accuracy of 74.5%. While the accuracy, TPR, and TNR are roughly balanced for Fastron FK, Fastron RBF’s TPR is much higher than its accuracy and TNR because it prioritizes the label over the label. While biasing toward the label yields a more conservative model, it also reduces the volume of the valid region of C-space and increases search time during sampling-based motion planning, which we will see is the case for Fastron RBF in the next section.
Fig. 5 shows typical failure cases using Fastron FK and Fastron RBF, where the true collision status is but the Fastron models incorrectly predict . Using the RBF kernel, the robot often significantly intersects with the workspace obstacle, especially at its more distal links. Using the FK kernel, the robot usually slightly intersects with the workspace obstacles in its failure cases. A slight intersection with workspace obstacles is less severe than blatantly penetrating the obstacle, demonstrating the benefit of comparing control points in the FK kernel over comparing joint values directly with the RBF kernel.
In an additional experiment, we increase the number of obstacles to see the effect on query times. Fig. 7 shows the query times for each method with respect to the number of workspace obstacles for up to 50 randomly-placed obstacles. We can see that the geometry-based methods, GJK and FCL, increase in query times as the number of obstacles increase. On the other hand, Fastron-based query times first increase and then decrease because fewer support points are required when one collision status is more prevalent than the other.
Iv-B Motion Planning
We apply Fastron FK to a broad collection of the sampling-based motion planning algorithms implemented in OMPL . We select RRT , RRT-Connect , SBL , RRT* , FMT* , and Informed-RRT*  to demonstrate each collision checking method’s performance. RRT, its bidirectional variant RRT-Connect, and SBL are probabilistically complete motion planners  that terminate once a path between start and goal is found. SBL includes lazy collision checking, which only calls the collision check routine when absolutely necessary. The other planners are optimizing planners that attempt to determine the shortest collision-free path between start and goal. Rather than letting the optimizing planners run until timeout (which would cause all planning times to be equal), we stop these planners once an initial feasible path has been found. While these paths may not be close to optimal, we include the average lengths of these initial paths in the plot of results.
We use similar environments as used in Section IV-A, but the obstacle is sized such that the arm must always move around it. The start and goal configurations are selected to be on opposite sides of the workspace obstacle. Each collision checker is applied to each motion planner for 50 trials, where a trial consists of up to 30 random movements of the workspace obstacle, and the motion planner is repeated 10 times for each obstacle position. When using the approximate collision checkers, we verify and repair (if necessary) each plan using FCL by replanning with FCL on the invalid part of the plan. The resulting motion plan is thus guaranteed to be collision-free according to FCL.
The average motion planning timings are shown in Fig. 6. Even with the verify and repair steps, Fastron FK provides the fastest motion plans, providing up to 3 times faster collision-free motion plans compared to when using FCL. On the other hand, Fastron RBF often struggles to find a feasible path because it overestimates the subspace, causing its planning time to often be large or hit the time limit. Furthermore, the repair times for Fastron RBF are significantly larger than those for Fastron FK and GJK, illustrating that Fastron RBF often yields invalid paths.
In this paper we introduce the FK kernel, and we use it with the Fastron algorithm for proxy collision detection for manipulator arms. The FK kernel utilizes control points placed at each joint in a kinematic chain to better represent the relation between C-space and workspace. The FK kernel is positive definite, which guarantees that the Fastron algorithm will provide a model with positive margin for a dataset in a finite number of iterations.
Compared to the previously-used RBF kernel, the FK kernel required 8 times fewer support points to represent the configuration space model and allowed 2 times faster proxy collision detection. Prediction accuracy is significantly higher with the FK kernel (96%) than with the RBF kernel (75%). The FK kernel allowed proxy collision checking to be 9 times faster and motion planning to be up to 3 times faster than when using geometric methods.
Future work includes utilization of GPU parallelization for faster kernel evaluation and proxy collision checking and applications of the FK kernel in other kernel-based methods.
-  (2005) Principles of Robot Motion: Theory, Algorithms, and Implementation. The MIT Press. Cited by: §I-B2, §I-B2, §I, §IV-B.
-  (1995) Support-Vector Networks. Machine Learning 20 (3), pp. 273–297. External Links: Cited by: §III-B.
-  (2019) Learning-Based Proxy Collision Detection for Robot Motion Planning Applications. arXiv preprint arXiv:1902.08164v1 [cs], pp. 1–19. External Links: Cited by: §I-A, §I-B1, §I-B1, §III-B, §III-B, Fig. 4, Fig. 7.
-  (2014) Sampling-Based Robot Motion Planning: A Review. IEEE Access 2, pp. 56–77. External Links: Cited by: §I.
Informed RRT *: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2997–3004. External Links: Cited by: §IV-B.
-  (1988) A Fast Procedure for Computing the Distance Between Complex Objects in Three-Dimensional Space. IEEE Journal on Robotics and Automation 4 (2), pp. 193–203. External Links: Cited by: Fig. 4, Fig. 7, §IV.
-  (2008) Kernel methods in machine learning. Annals of Statistics 36 (3), pp. 1171–1220. External Links: Cited by: §II-B.
-  (2016) Learning high-dimensional Mixture Models for fast collision detection in Rapidly-Exploring Random Trees. Proceedings - IEEE International Conference on Robotics and Automation 2016-June, pp. 63–69. External Links: Cited by: §I-B1, §I-B1.
-  (2011) Sampling-based Algorithms for Optimal Motion Planning. International Journal of Robotics Research 30 (7), pp. 1–76. External Links: Cited by: §IV-B.
-  (1985) Real-time obstacle avoidance for manipulators and mobile robots. In IEEE International Conference on Robotics and Automation, pp. 500–505. Cited by: §I-B2.
-  (2000) RRT-Connect : An Efficient Approach to Single-Query Path Planning. In International Conference on Robotics and Automation, pp. 995–1001. External Links: Cited by: §IV-B.
-  (2008) Rapidly-Exploring Random Trees: A New Tool for Path Planning. Animal Genetics 39 (5), pp. 561–563. Cited by: §IV-B.
-  (1983) Spatial Planning : A Configuration Space Approach. IEEE Transactions on Computers 32 (2). Cited by: §I.
Mercer’s Theorem, Feature Maps, and Smoothing.
International Conference on Computational Learning Theory, pp. 154–168. External Links: Cited by: §II-B.
-  (2012) FCL: A general purpose library for collision and proximity queries. Proceedings - IEEE International Conference on Robotics and Automation, pp. 3859–3866. External Links: Cited by: Fig. 7, §IV.
-  (2015) Efficient Configuration Space Construction and Optimization for Motion Planning. Engineering 1 (1), pp. 046–057. External Links: Cited by: §I-B1, §I-B1, §I.
-  (2016) Fast probabilistic collision checking for sampling-based motion planning using locality-sensitive hashing. International Journal of Robotics Research 35 (12), pp. 1477–1496. External Links: Cited by: §I-B1, §I-B1.
-  (2018) RelaxedIK : Real-time Synthesis of Accurate and Feasible Robot Arm Motion. In Robotics: Science and Systems, Cited by: §I-B2.
-  (2006) Gaussian Processes for Machine Learning. The MIT Press. External Links: Cited by: §II-B, §II-B.
-  (2011) Robotics and Computer-Integrated Manufacturing A comparison between the Denavit – Hartenberg and the screw-based methods used in kinematic modeling of robot manipulators $. Robotics and Computer Integrated Manufacturing 27 (4), pp. 723–728. External Links: Cited by: §II-A.
-  (2003) A Single-Query Bi-Directional Probabilistic Roadmap. In The Tenth International Symposium on Robotics Research, pp. 403–417. Cited by: §IV-B.
-  (2015) An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional Motion Planning. In IEEE/RSJ International Conference on Intelligent Robots Systems, External Links: Cited by: §IV-B.
-  MoveIt!. External Links: Cited by: §IV.
-  (2012) The Open Motion Planning Library. IEEE Robotics & Automation Magazine (December), pp. 72–82. Cited by: §IV-B.