Reducing the Collision Checking Time in Cluttered Environment for Sampling-Based Motion Planning

In this paper, we discuss the field of sampling-based motion planning. In contrast to methods that construct boundary representations of configuration space obstacles, samplingbased methods use only information from a collision detector as they search the configuration space. The simplicity of this approach, along with increases in computation power and the development of efficient collision detection algorithms, has resulted in the introduction of a number of powerful motion planning algorithms, capable of solving challenging problems with many degrees of freedom. First, we trace how sampling-based motion planning has developed. We then discuss a variety of important issues for sampling-based motion planning, including uniform and regular sampling, topological issues, and search philosophies. Finally, we address important issues regarding the role of randomization in sampling-based motion planning.

We present a novel approach to perform fast probabilistic collision checking in high-dimensional configuration spaces to accelerate the performance of sampling-based motion planning. Our formulation stores the results of prior collision queries, and then uses such information to predict the collision probability for a new configuration sample. In particular, we perform an approximate k-NN (k-nearest neighbor) search to find prior query samples that are closest to the new query configuration. The new query sample’s collision status is then estimated according to the collision checking results of these prior query samples, based on the fact that nearby configurations are likely to have the same collision status. We use locality-sensitive hashing techniques with sub-linear time complexity for approximate k-NN queries. We evaluate the benefit of our probabilistic collision checking approach by integrating it with a wide variety of sampling-based motion planners, including PRM (Probabilistic roadmaps), lazyPRM, RRT Rapidly exploring random trees, and RRT*. Our method can improve these planners in various manners, such as accelerating the local path validation, or computing an efficient order for the graph search on the roadmap. Experiments on a set of benchmarks demonstrate the performance of our method, and we observe up to 2x speedup in the performance of planners on rigid and articulated robots.

Sampling-based Motion Planners (SMPs) have become increasingly popular as they provide collision-free path solutions regardless of obstacle geometry in a given environment. However, their computational complexity increases significantly with the dimensionality of the motion planning problem. Adaptive sampling is one of the ways to speed up SMPs by sampling a particular region of a configuration space that is more likely to contain an optimal path solution. Although there are a wide variety of algorithms for adaptive sampling, they rely on hand-crafted heuristics; furthermore, their performance decreases significantly in high-dimensional spaces. In this paper, we present a neural network-based adaptive sampler for motion planning called Deep Sampling-based Motion Planner (DeepSMP). DeepSMP generates samples for SMPs and enhances their overall speed significantly while exhibiting efficient scalability to higher-dimensional problems. DeepSMP's neural architecture comprises of a Contractive AutoEncoder which encodes given workspaces directly from a raw point cloud data, and a Dropout-based stochastic deep feedforward neural network which takes the workspace encoding, start and goal configuration, and iteratively generates feasible samples for SMPs to compute end-to-end collision-free optimal paths. DeepSMP is not only consistently computationally efficient in all tested environments but has also shown remarkable generalization to completely unseen environments. We evaluate DeepSMP on multiple planning problems including planning of a point-mass robot, rigid-body, 6-link robotic manipulator in various 2D and 3D environments. The results show that on average our method is at least 7 times faster in point-mass and rigid-body case and about 28 times faster in 6-link robot case than the existing state-of-the-art.

Lazy collision checking has been proposed to reduce the computational burden of collision checking which is considered as a major computational bottleneck in sampling-based motion planning. Unfortunately, in complex environments with many obstacles, lazy collision checking can cause an excessive amount of optimistic thrashing problems and significantly degrade the performance of the planner. In this paper, we present an adaptive lazy collision checking method to alleviate the optimistic thrashing, thus broaden the applicability. Our method delays collision checking on the regions predicted to be in the configuration free space, while checking early on the other regions to reduce the optimistic thrashing. To identify such regions, we adopt a configuration free space approximation represented by a set of hyperspheres which can be constructed without significant proximity computation. To demonstrate benefits of our approach we have compared against prior methods including RRT*, PRM* and lazy PRM* across benchmarks with varying dimensions. Overall, our method shows meaningful performance improvement up to three times higher in terms of convergence speed over the tested methods. We also discuss properties and theoretical analysis of the proposed algorithm.

A fast and accurate compound collision detector for RRT motion planning

The complexity of nearest-neighbor search dominates the asymptotic running time of many sampling-based motion-planning algorithms. However, collision detection is often considered to be the computational bottleneck in practice. Examining various asymptotically optimal planning algorithms, we characterize settings, which we call NN-sensitive, in which the practical computational role of nearest-neighbor search is far from being negligible, i.e., the portion of running time taken up by nearest-neighbor search is comparable to, or sometimes even greater than the portion of time taken up by collision detection. This reinforces and substantiates the claim that motion-planning algorithms could significantly benefit from efficient and possibly specially-tailored nearest-neighbor data structures. The asymptotic (near) optimality of these algorithms relies on a prescribed connection radius, defining a ball around a configuration q, such that q needs to be connected to all other configurations in that ball. To facilitate our study, we show how to adapt this radius to non-Euclidean spaces, which are prevalent in motion planning. This technical result is of independent interest, as it enables to compare the radial-connection approach with the common alternative, namely, connecting each configuration to its k nearest neighbors (K-NN). Indeed, as we demonstrate, there are scenarios where using the radial connection scheme, a solution path of a specific cost is produced ten-fold (and more) faster than with K-NN.

Chapter 8 - Probabilistic roadmap

We discuss the optimality and computational efficiency of sampling based motion planning (SBMP), which calculates dynamically precise and approximately optimal state transitions using arbitrarily selected nonlinear control system models. To ensure high optimality and computational efficiency, SBMP requires an approximately uniform state sampling function, though the non-linearity of the system models does not allow a perfect function. We propose linear prediction-based uniform state sampling (LPUSS) that samples approximately uniform state points while ensuring a dynamically correct state transition profile with a small calculation cost. LPUSS samples a state by using the given non-linear control system model after determining the input values by using a local linear transition model. We developed a mechanical motion planning system using LPUSS, articulated body algorithm, and parallel computing techniques. To validate LPUSS, we conducted experiments on double, triple, and sixtuple inverted pendulum models. LPUSS showed better optimality and computational efficiency with the double and triple inverted pendulum models, compared with randomized kinodynamic planning (RKP), which is based on rapid random tree (RRT), and our previously proposed rapid semi-optimal motion-planning method in which state sampling is based on uniform inputs. In particular, compared with our previous method, LPUSS was respectively 130 times and 3,000 times faster on double and triple inverted pendulum models under the condition of the same optimality. LPUSS found an approximately optimal swing up motion for the sixtuple inverted pendulum model within 40 minutes. According to our survey, there is no other optimization method that is applicable to higher than quadruple inverted pendulum models with standard constraints.

Kernel functions may be used in robotics for comparing different poses of a robot, such as in collision checking, inverse kinematics, and motion planning. These comparisons provide distance metrics often based on joint space measurements and are performed hundreds or thousands of times a second, continuously for changing environments. We introduce a new kernel function based on forward kinematics (FK) to compare robot manipulator configurations. We integrate our new FK kernel into our proxy collision checker, Fastron, that previously showed significant speed improvements to collision checking and motion planning. With the new FK kernel, we realize a two-fold speedup in proxy collision check speed, 8 times less memory, and a boost in classification accuracy from 74% to over 95% for a 7 degrees-of-freedom robot arm compared to the previously-used radial basis function kernel. Compared to state-of-the-art geometric collision checkers, with the FK kernel, collision checks are now 9 times faster. To show the broadness of the approach, we apply Fastron FK in OMPL across a wide variety of motion planners, showing unanimously faster robot planning.

Sampling-based motion planning techniques have emerged as an efficient algorithmic paradigm for solving complex motion planning problems. These approaches use a set of probing samples to construct an implicit graph representation of the robot's state space, allowing arbitrarily accurate representations as the number of samples increases to infinity. In practice, however, solution trajectories only rely on a few critical states, often defined by structure in the state space (e.g., doorways). In this work we propose a general method to identify these critical states via graph-theoretic techniques (betweenness centrality) and learn to predict criticality from only local environment features. These states are then leveraged more heavily via global connections within a hierarchical graph, termed Critical Probabilistic Roadmaps. Critical PRMs are demonstrated to achieve up to three orders of magnitude improvement over uniform sampling, while preserving the guarantees and complexity of sampling-based motion planning. A video is available at https://youtu.be/AYoD-pGd9ms.

To compute collision-free and dynamically-feasibile trajectories that satisfy high-level specifications given in a planning-domain definition language, this paper proposes to combine sampling-based motion planning with symbolic action planning. The proposed approach, Sampling-based Motion and Symbolic Action Planner (SMAP), leverages from sampling-based motion planning the underlying idea of searching for a solution trajectory by selectively sampling and exploring the continuous space of collision-free and dynamically-feasible motions. Drawing from AI, SMAP uses symbolic action planning to identify actions and regions of the continuous space that sampling-based motion planning can further explore to significantly advance the search. The planning layers interact with each-other through estimates on the utility of each action, which are computed based on information gathered during the search. Simulation experiments with dynamical models of vehicles carrying out tasks given by high-level STRIPS specifications provide promising initial validation, showing that SMAP efficiently solves challenging problems.

We propose the combination of techniques that solve multiple queries for motion planning problems with single query planners in a motion planning framework that can be efficiently parallelized. In multiple query motion planning, a data structure is built during a preprocessing phase in order to quickly respond to on-line queries. Alternatively, in single query planning, there is no preprocessing phase and all computations occur during query resolution. This paper shows how to effectively combine a powerful sample-based method primarily designed for multiple query planning (the Probabilistic Roadmap Method - PRM) with sample-based tree methods that were primarily designed for single query planning (such as Expansive Space Trees, Rapidly Exploring Random Trees, and others). Our planner, which we call the Probabilistic Roadmap of Trees (PRT), uses a tree algorithm as a subroutine for PRM. The nodes of the PRM roadmap are now trees. We take advantage of the very powerful sampling schemes of recent tree planners to populate our roadmaps. The combined sampling scheme is in the spirit of the non-uniform sampling and refinement techniques employed in earlier work on PRM. PRT not only achieves a smooth spectrum between multiple query and single query planning but it combines advantages of both. We present experiments which show that PRT is capable of solving problems that cannot be addressed efficiently with PRM or single-query planners. A key advantage of PRT is that it is significantly more decoupled than PRM and sample-based tree planners. Using this property, we designed and implemented a parallel version of PRT. Our experiments show that PRT distributes well and can easily solve high dimensional problems that exhaust resources available to single machines.

Optimal Mesh Algorithms for the Voronoi Diagram of Line Segments and Motion Planning in the Plane

Collision detection is a fundamental geometric tool for sampling-based motion planners. On the contrary, collision prediction for the scenarios that obstacle’s motion is unknown is still in its infancy. This paper proposes a new approach to predict collision by assuming that obstacles are adversarial. Our new tool advances collision prediction beyond the translational and disc robots; arbitrary polygons with rotation can be used to better represent obstacles and provide tighter bound on predicted collision time. Comparing to an online motion planner that replans periodically at fixed time interval, our experimental results provide strong evidences that our method significantly reduces the number of re-plannings while maintaining higher success rate of finding a valid path.

Asymptotically-optimal sampling-based motion planners, like RRT*, perform vast amounts of collision checking, and are hence rather slow to converge in complex problems where collision checking is relatively expensive. This paper presents two novel motion planners, Lazy-PRM* and Lazy-RRG*, that eliminate the majority of collision checks using a lazy strategy. They are sampling-based, any-time, and asymptotically complete algorithms that grow a network of feasible vertices connected by edges. Edges are not immediately checked for collision, but rather are checked only when a better path to the goal is found. This strategy avoids checking the vast majority of edges that have no chance of being on an optimal path. Experiments show that the new methods converge toward the optimum substantially faster than existing planners on rigid body path planning and robot manipulation problems.