Incremental Sampling-based Algorithms for Optimal Motion Planning

During the last decade, incremental sampling-based motion planning algorithms, such as the Rapidly-exploring Random Trees (RRTs) have been shown to work well in practice and to possess theoretical guarantees such as probabilistic completeness. However, no theoretical bounds on the quality of the solution obtained by these algorithms have been established so far. The first contribution of this paper is a negative result: it is proven that, under mild technical conditions, the cost of the best path in the RRT converges almost surely to a non-optimal value. Second, a new algorithm is considered, called the Rapidly-exploring Random Graph (RRG), and it is shown that the cost of the best path in the RRG converges to the optimum almost surely. Third, a tree version of RRG is introduced, called the RRT$^*$ algorithm, which preserves the asymptotic optimality of RRG while maintaining a tree structure like RRT. The analysis of the new algorithms hinges on novel connections between sampling-based motion planning algorithms and the theory of random geometric graphs. In terms of computational complexity, it is shown that the number of simple operations required by both the RRG and RRT$^*$ algorithms is asymptotically within a constant factor of that required by RRT.Comment: 20 pages, 10 figures, this manuscript is submitted to the International Journal of Robotics Research, a short version is to appear at the 2010 Robotics: Science and Systems Conference

Sampling-based Algorithms for Optimal Motion Planning

During the last decade, sampling-based path planning algorithms, such as Probabilistic RoadMaps (PRM) and Rapidly-exploring Random Trees (RRT), have been shown to work well in practice and possess theoretical guarantees such as probabilistic completeness. However, little effort has been devoted to the formal analysis of the quality of the solution returned by such algorithms, e.g., as a function of the number of samples. The purpose of this paper is to fill this gap, by rigorously analyzing the asymptotic behavior of the cost of the solution returned by stochastic sampling-based algorithms as the number of samples increases. A number of negative results are provided, characterizing existing algorithms, e.g., showing that, under mild technical conditions, the cost of the solution returned by broadly used sampling-based algorithms converges almost surely to a non-optimal value. The main contribution of the paper is the introduction of new algorithms, namely, PRM* and RRT*, which are provably asymptotically optimal, i.e., such that the cost of the returned solution converges almost surely to the optimum. Moreover, it is shown that the computational complexity of the new algorithms is within a constant factor of that of their probabilistically complete (but not asymptotically optimal) counterparts. The analysis in this paper hinges on novel connections between stochastic sampling-based path planning algorithms and the theory of random geometric graphs.Comment: 76 pages, 26 figures, to appear in International Journal of Robotics Researc

Sampling-based optimal kinodynamic planning with motion primitives

This paper proposes a novel sampling-based motion planner, which integrates in RRT* (Rapidly exploring Random Tree star) a database of pre-computed motion primitives to alleviate its computational load and allow for motion planning in a dynamic or partially known environment. The database is built by considering a set of initial and final state pairs in some grid space, and determining for each pair an optimal trajectory that is compatible with the system dynamics and constraints, while minimizing a cost. Nodes are progressively added to the tree {of feasible trajectories in the RRT* by extracting at random a sample in the gridded state space and selecting the best obstacle-free motion primitive in the database that joins it to an existing node. The tree is rewired if some nodes can be reached from the new sampled state through an obstacle-free motion primitive with lower cost. The computationally more intensive part of motion planning is thus moved to the preliminary offline phase of the database construction at the price of some performance degradation due to gridding. Grid resolution can be tuned so as to compromise between (sub)optimality and size of the database. The planner is shown to be asymptotically optimal as the grid resolution goes to zero and the number of sampled states grows to infinity

Optimal Sampling-Based Motion Planning under Differential Constraints: the Driftless Case

Motion planning under differential constraints is a classic problem in robotics. To date, the state of the art is represented by sampling-based techniques, with the Rapidly-exploring Random Tree algorithm as a leading example. Yet, the problem is still open in many aspects, including guarantees on the quality of the obtained solution. In this paper we provide a thorough theoretical framework to assess optimality guarantees of sampling-based algorithms for planning under differential constraints. We exploit this framework to design and analyze two novel sampling-based algorithms that are guaranteed to converge, as the number of samples increases, to an optimal solution (namely, the Differential Probabilistic RoadMap algorithm and the Differential Fast Marching Tree algorithm). Our focus is on driftless control-affine dynamical models, which accurately model a large class of robotic systems. In this paper we use the notion of convergence in probability (as opposed to convergence almost surely): the extra mathematical flexibility of this approach yields convergence rate bounds - a first in the field of optimal sampling-based motion planning under differential constraints. Numerical experiments corroborating our theoretical results are presented and discussed

Near-Optimal Motion Planning Algorithms Via A Topological and Geometric Perspective

Motion planning is a fundamental problem in robotics, which involves finding a path for an autonomous system, such as a robot, from a given source to a destination while avoiding collisions with obstacles. The properties of the planning space heavily influence the performance of existing motion planning algorithms, which can pose significant challenges in handling complex regions, such as narrow passages or cluttered environments, even for simple objects. The problem of motion planning becomes deterministic if the details of the space are fully known, which is often difficult to achieve in constantly changing environments. Sampling-based algorithms are widely used among motion planning paradigms because they capture the topology of space into a roadmap. These planners have successfully solved high-dimensional planning problems with a probabilistic-complete guarantee, i.e., it guarantees to find a path if one exists as the number of vertices goes to infinity. Despite their progress, these methods have failed to optimize the sub-region information of the environment for reuse by other planners. This results in re-planning overhead at each execution, affecting the performance complexity for computation time and memory space usage. In this research, we address the problem by focusing on the theoretical foundation of the algorithmic approach that leverages the strengths of sampling-based motion planners and the Topological Data Analysis methods to extract intricate properties of the environment. The work contributes a novel algorithm to overcome the performance shortcomings of existing motion planners by capturing and preserving the essential topological and geometric features to generate a homotopy-equivalent roadmap of the environment. This roadmap provides a mathematically rich representation of the environment, including an approximate measure of the collision-free space. In addition, the roadmap graph vertices sampled close to the obstacles exhibit advantages when navigating through narrow passages and cluttered environments, making obstacle-avoidance path planning significantly more efficient. The application of the proposed algorithms solves motion planning problems, such as sub-optimal planning, diverse path planning, and fault-tolerant planning, by demonstrating the improvement in computational performance and path quality. Furthermore, we explore the potential of these algorithms in solving computational biology problems, particularly in finding optimal binding positions for protein-ligand or protein-protein interactions. Overall, our work contributes a new way to classify routes in higher dimensional space and shows promising results for high-dimensional robots, such as articulated linkage robots. The findings of this research provide a comprehensive solution to motion planning problems and offer a new perspective on solving computational biology problems

On Randomized Path Coverage of Configuration Spaces

We present a sampling-based algorithm that generates a set of locally-optimal paths that differ in visibility

Robot Motion Planning Under Topological Constraints

My thesis addresses the the problem of manipulation using multiple robots with cables. I study how robots with cables can tow objects in the plane, on the ground and on water, and how they can carry suspended payloads in the air. Specifically, I focus on planning optimal trajectories for robots. Path planning or trajectory generation for robotic systems is an active area of research in robotics. Many algorithms have been developed to generate path or trajectory for different robotic systems. One can classify planning algorithms into two broad categories. The first one is graph-search based motion planning over discretized configuration spaces. These algorithms are complete and quite efficient for finding optimal paths in cluttered 2-D and 3-D environments and are widely used [48]. The other class of algorithms are optimal control based methods. In most cases, the optimal control problem to generate optimal trajectories can be framed as a nonlinear and non convex optimization problem which is hard to solve. Recent work has attempted to overcome these shortcomings [68]. Advances in computational power and more sophisticated optimization algorithms have allowed us to solve more complex problems faster. However, our main interest is incorporating topological constraints. Topological constraints naturally arise when cables are used to wrap around objects. They are also important when robots have to move one way around the obstacles rather than the other way around. Thus I consider the optimal trajectory generation problem under topological constraints, and pursue problems that can be solved in finite-time, guaranteeing global optimal solutions. In my thesis, I first consider the problem of planning optimal trajectories around obstacles using optimal control methodologies. I then present the mathematical framework and algorithms for multi-robot topological exploration of unknown environments in which the main goal is to identify the different topological classes of paths. Finally, I address the manipulation and transportation of multiple objects with cables. Here I consider teams of two or three ground robots towing objects on the ground, two or three aerial robots carrying a suspended payload, and two boats towing a boom with applications to oil skimming and clean up. In all these problems, it is important to consider the topological constraints on the cable configurations as well as those on the paths of robot. I present solutions to the trajectory generation problem for all of these problems