Multilevel Motion Planning: A Fiber Bundle Formulation

Motion planning problems involving high-dimensional state spaces can often be solved significantly faster by using multilevel abstractions. While there are various ways to formally capture multilevel abstractions, we formulate them in terms of fiber bundles, which allows us to concisely describe and derive novel algorithms in terms of bundle restrictions and bundle sections. Fiber bundles essentially describe lower-dimensional projections of the state space using local product spaces. Given such a structure and a corresponding admissible constraint function, we can develop highly efficient and optimal search-based motion planning methods for high-dimensional state spaces. Our contributions are the following: We first introduce the terminology of fiber bundles, in particular the notion of restrictions and sections. Second, we use the notion of restrictions and sections to develop novel multilevel motion planning algorithms, which we call QRRT* and QMP*. We show these algorithms to be probabilistically complete and almost-surely asymptotically optimal. Third, we develop a novel recursive path section method based on an L1 interpolation over path restrictions, which we use to quickly find feasible path sections. And fourth, we evaluate all novel algorithms against all available OMPL algorithms on benchmarks of eight challenging environments ranging from 21 to 100 degrees of freedom, including multiple robots and nonholonomic constraints. Our findings support the efficiency of our novel algorithms and the benefit of exploiting multilevel abstractions using the terminology of fiber bundles.Comment: Submitted to IJR

Path Planning With Adaptive Dimensionality

Path planning quickly becomes computationally hard as the dimensionality of the state-space increases. In this paper, we present a planning algorithm intended to speed up path planning for high-dimensional state-spaces such as robotic arms. The idea behind this work is that while planning in a highdimensional state-space is often necessary to ensure the feasibility of the resulting path, large portions of the path have a lower-dimensional structure. Based on this observation, our algorithm iteratively constructs a state-space of an adaptive dimensionality–a state-space that is high-dimensional only where the higher dimensionality is absolutely necessary for finding a feasible path. This often reduces drastically the size of the state-space, and as a result, the planning time and memory requirements. Analytically, we show that our method is complete and is guaranteed to find a solution if one exists, within a specified suboptimality bound. Experimentally, we apply the approach to 3D vehicle navigation (x, y, heading), and to a 7 DOF robotic arm on the Willow Garage’s PR2 robot. The results from our experiments suggest that our method can be substantially faster than some of the state-ofthe-art planning algorithms optimized for those tasks

The Ariadne's Clew Algorithm

We present a new approach to path planning, called the "Ariadne's clew algorithm". It is designed to find paths in high-dimensional continuous spaces and applies to robots with many degrees of freedom in static, as well as dynamic environments - ones where obstacles may move. The Ariadne's clew algorithm comprises two sub-algorithms, called Search and Explore, applied in an interleaved manner. Explore builds a representation of the accessible space while Search looks for the target. Both are posed as optimization problems. We describe a real implementation of the algorithm to plan paths for a six degrees of freedom arm in a dynamic environment where another six degrees of freedom arm is used as a moving obstacle. Experimental results show that a path is found in about one second without any pre-processing

The Ariadne's Clew Algorithm

We present a new approach to path planning, called the ``Ariadne's clew algorithm''. It is designed to find paths in high-dimensional continuous spaces and applies to robots with many degrees of freedom in static, as well as dynamic environments --- ones where obstacles may move. The Ariadne's clew algorithm comprises two sub-algorithms, called SEARCH and EXPLORE, applied in an interleaved manner. EXPLORE builds a representation of the accessible space while SEARCH looks for the target. Both are posed as optimization problems. We describe a real implementation of the algorithm to plan paths for a six degrees of freedom arm in a dynamic environment where another six degrees of freedom arm is used as a moving obstacle. Experimental results show that a path is found in about one second without any pre-processing

A Unifying Variational Framework for Gaussian Process Motion Planning

To control how a robot moves, motion planning algorithms must compute paths in high-dimensional state spaces while accounting for physical constraints related to motors and joints, generating smooth and stable motions, avoiding obstacles, and preventing collisions. A motion planning algorithm must therefore balance competing demands, and should ideally incorporate uncertainty to handle noise, model errors, and facilitate deployment in complex environments. To address these issues, we introduce a framework for robot motion planning based on variational Gaussian Processes, which unifies and generalizes various probabilistic-inference-based motion planning algorithms. Our framework provides a principled and flexible way to incorporate equality-based, inequality-based, and soft motion-planning constraints during end-to-end training, is straightforward to implement, and provides both interval-based and Monte-Carlo-based uncertainty estimates. We conduct experiments using different environments and robots, comparing against baseline approaches based on the feasibility of the planned paths, and obstacle avoidance quality. Results show that our proposed approach yields a good balance between success rates and path quality

Sensory Steering for Sampling-Based Motion Planning

Sampling-based algorithms offer computationally efficient, practical solutions to the path finding problem in high-dimensional complex configuration spaces by approximately capturing the connectivity of the underlying space through a (dense) collection of sample configurations joined by simple local planners. In this paper, we address a long-standing bottleneck associated with the difficulty of finding paths through narrow passages. Whereas most prior work considers the narrow passage problem as a sampling issue (and the literature abounds with heuristic sampling strategies) very little attention has been paid to the design of new effective local planners. Here, we propose a novel sensory steering algorithm for sampling- based motion planning that can “feel” a configuration space locally and significantly improve the path planning performance near difficult regions such as narrow passages. We provide computational evidence for the effectiveness of the proposed local planner through a variety of simulations which suggest that our proposed sensory steering algorithm outperforms the standard straight-line planner by significantly increasing the connectivity of random motion planning graphs. For more information: Kod*la

On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages

We extend our study of Motion Planning via Manifold Samples (MMS), a general algorithmic framework that combines geometric methods for the exact and complete analysis of low-dimensional configuration spaces with sampling-based approaches that are appropriate for higher dimensions. The framework explores the configuration space by taking samples that are entire low-dimensional manifolds of the configuration space capturing its connectivity much better than isolated point samples. The contributions of this paper are as follows: (i) We present a recursive application of MMS in a six-dimensional configuration space, enabling the coordination of two polygonal robots translating and rotating amidst polygonal obstacles. In the adduced experiments for the more demanding test cases MMS clearly outperforms PRM, with over 20-fold speedup in a coordination-tight setting. (ii) A probabilistic completeness proof for the most prevalent case, namely MMS with samples that are affine subspaces. (iii) A closer examination of the test cases reveals that MMS has, in comparison to standard sampling-based algorithms, a significant advantage in scenarios containing high-dimensional narrow passages. This provokes a novel characterization of narrow passages which attempts to capture their dimensionality, an attribute that had been (to a large extent) unattended in previous definitions.Comment: 20 page

Generalizing Informed Sampling for Asymptotically Optimal Sampling-based Kinodynamic Planning via Markov Chain Monte Carlo

Asymptotically-optimal motion planners such as RRT* have been shown to incrementally approximate the shortest path between start and goal states. Once an initial solution is found, their performance can be dramatically improved by restricting subsequent samples to regions of the state space that can potentially improve the current solution. When the motion planning problem lies in a Euclidean space, this region $X_{inf}$, called the informed set, can be sampled directly. However, when planning with differential constraints in non-Euclidean state spaces, no analytic solutions exists to sampling $X_{inf}$ directly. State-of-the-art approaches to sampling $X_{inf}$ in such domains such as Hierarchical Rejection Sampling (HRS) may still be slow in high-dimensional state space. This may cause the planning algorithm to spend most of its time trying to produces samples in $X_{inf}$ rather than explore it. In this paper, we suggest an alternative approach to produce samples in the informed set $X_{inf}$ for a wide range of settings. Our main insight is to recast this problem as one of sampling uniformly within the sub-level-set of an implicit non-convex function. This recasting enables us to apply Monte Carlo sampling methods, used very effectively in the Machine Learning and Optimization communities, to solve our problem. We show for a wide range of scenarios that using our sampler can accelerate the convergence rate to high-quality solutions in high-dimensional problems