This paper presents an equivalence between feasible kinodynamic planning and optimal kinodynamic planning, in that any optimal planning problem can be transformed into a series of feasible planning problems in a state-cost space whose solutions approach the optimum. This transformation gives rise to a meta-algorithm that produces an asymptotically optimal planner, given any feasible kinodynamic planner as a subroutine. The meta-algorithm is proven to be asymptotically optimal, and a formula is derived relating expected running time and solution suboptimality. It is directly applicable to a wide range of optimal planning problems because it does not resort to the use of steering functions or numerical boundary-value problem solvers. On a set of benchmark problems, it is demonstrated to perform, using the EST and RRT algorithms as subroutines, at a superior or comparable level to related planners.
The prior work with closest relation to ours in terms of generality of applicability is the Sparse-Stable-RRT planner @cite_23 . Like our work, it avoids the use of a steering function entirely and samples directly in control space. Approximate rewiring is performed by allowing connections to points that are near enough'' according to a state-space distance metric. This scheme was proven to satisfy asymptotic near-optimality, which is the property of converging toward a path with bounded suboptimality @cite_10 . In a more recent paper, the same authors have extended it to an asymptotically-optimal planner, SST*, by progressively shrinking nearness threshold parameters @cite_12 . However, Sparse-Stable-RRT and SST* and have many parameters to tune, and our experiments suggest that AO- @math planners in general outperform both planners.
@cite_10 @cite_12 @cite_23
1986004895 1969907553
Recent motion planners, such as RRT*, that achieve asymptotic optimality require a local planner, which connects two states with a trajectory. For systems with dynamics, the local planner corresponds to a two-point boundary value problem (BVP) solver, which is not always available. Furthermore, asymptotically optimal solutions tend to increase computational costs relative to alternatives, such as RRT, that focus on feasibility. This paper describes a sampling-based solution with the following desirable properties: a) it does not require a BVP solver but only uses a forward propagation model, b) it employs a single propagation per iteration similar to RRT, making it very efficient, c) it is asymptotically near-optimal, and d) provides a sparse data structure for answering path queries, which further improves computational performance. Simulations on prototypical dynamical systems show the method is able to improve the quality of feasible solutions over time and that it is computationally efficient. Sampling-based algorithms have proven practical in solving motion planning challenges in relatively high-dimensional instances in geometrically complex workspaces. Early work focused on quickly returning feasible solutions. Only recently was it shown under which conditions these algorithms asymptotically return optimal or near-optimal solutions. These methods yield desired properties only in an asymptotic fashion, i.e., the properties are attained after infinite computation time. This work studies the finite-time properties of sampling-based planners in terms of path quality. The focus is on roadmap-based methods, due to their simplicity. This work illustrates that existing sampling-based planners which construct roadmaps in an asymptotically (near-)optimal manner exhibit a “probably near-optimal” property in finite time. This means that it is possible to compute a confidence value, i.e. a probability, regarding the existence of upper bounds for the length of the path returned by the roadmap as a function of the number of configuration space samples. This property can result in useful tools for determining existence of solutions and a probabilistic stopping criterion for PRM-like methods. These properties are validated through experimental trials. Asymptotically optimal planners, such as PRM*, guarantee that solutions approach optimal as the number of iterations increases. Roadmaps with this property, however, may grow too large for storing on resource-constrained robots and for achieving efficient online query resolution. By relaxing optimality, asymptotically near-optimal planners produce sparser graphs by not including all edges. The idea stems from graph spanners, which produce sparse subgraphs that guarantee near-optimality. Existing asymptotically optimal and near-optimal planners, however, include all sampled configurations as roadmap nodes, meaning only infinite-size graphs have the desired properties. To address this limitation, this work describes SPARS, an algorithm that returns a sparse roadmap spanner. The method provides the following properties: (a) probabilistic completeness, (b) asymptotic near-optimality and (c) the probability of adding nodes to the spanner converges to zero as iterations increase. The last point suggests that finite-size data structures with asymptotic near-optimality in continuous spaces may indeed exist. The approach builds simultaneously a dense graph similar to PRM* and its roadmap spanner, meaning that upon construction an infinite-size graph is still needed asymptotically. An extension of SPARS is also presented, termed SPARS2, which removes the dependency on building a dense graph for constructing the sparse roadmap spanner and for which it is shown that the same desirable properties hold. Simulations for rigid-body motion planning show that algorithms for constructing sparse roadmap spanners indeed provide small data structures and result in faster query resolution. The rate of node addition is shown to decrease over time and practically the quality of solutions is considerably better than the theoretical bounds. Upon construction, the memory requirements of SPARS2 are significantly smaller but there is a small sacrifice in the size of the final spanner relative to SPARS. 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. Sampling based planners have become increasingly efficient in solving the problems of classical motion planning and its applications. In particular, techniques based on the rapidly-exploring random trees (RRTs) have generated highly successful single-query planners. Recently, a variant of this planner called dynamic-domain RRT was introduced by (2005). It relies on a new sampling scheme that improves the performance of the RRT approach on many motion planning problems. One of the drawbacks of this method is that it introduces a new parameter that requires careful tuning. In this paper we analyze the influence of this parameter and propose a new variant of the dynamic-domain RRT, which iteratively adapts the sampling domain for the Voronoi region of each node during the search process. This allows automatic tuning of the parameter and significantly increases the robustness of the algorithm. The resulting variant of the algorithm has been tested on several path planning problems. Efficient footstep planning for humanoid navigation through cluttered environments is still a challenging problem. Many obstacles create local minima in the search space, forcing heuristic planners such as A* to expand large areas. The goal of this work is to efficiently compute long, feasible footstep paths. For navigation, finding the optimal path initially is often not needed as it can be improved while walking. Thus, we propose anytime search-based planning using the anytime repairing A* (ARA*) and randomized A* (R*) planners. This allows to obtain efficient paths with provable suboptimality within short planning times. Opposed to completely randomized methods such as rapidly-exploring random trees (RRTs), these planners create paths that are goal-directed and guaranteed to be no more than a certain factor longer than the optimal solution. We thoroughly evaluated the planners in various scenarios using different heuristics. ARA* with the 2D Dijkstra heuristic yields fast and efficient solutions but its potential inadmissibility results in non-optimal paths for some scenarios. R*, on the other hand borrows ideas from RRTs, yields fast solutions, and is less dependent on a well-designed heuristic function. This allows it to avoid local minima and reduces the number of expanded states. Sampling-based optimal planners, such as RRT*, almost-surely converge asymptotically to the optimal solution, but have provably slow convergence rates in high dimensions. This is because their commitment to finding the global optimum compels them to prioritize exploration of the entire problem domain even as its size grows exponentially. Optimization techniques, such as CHOMP, have fast convergence on these problems but only to local optima. This is because they are exploitative, prioritizing the immediate improvement of a path even though this may not find the global optimum of nonconvex cost functions. Sampling-based algorithms such as the Rapidly-exploring Random Tree (RRT) have been recently proposed as an effective approach to computationally hard motion planning problem. However, while the RRT algorithm is known to be able to find a feasible solution quickly, there are no guarantees on the quality of such solution, e.g., with respect to a given cost functional. To address this limitation, the authors recently proposed a new algorithm, called RRT*, which ensures asymptotic optimality, i.e., almost sure convergence of the solution returned by the algorithm to an optimal solution, while maintaining the same properties of the standard RRT algorithm, both in terms of computation of feasible solutions, and of computational complexity. In this paper, the RRT* algorithm is extended to deal with differential constraints. A sufficient condition for asymptotic optimality is provided. It is shown that the RRT* algorithm equipped with any local steering procedure that satisfies this condition converges to an optimal solution almost surely. In particular, simple local steering procedures are provided for a Dubins' vehicle as well as a double integrator. Simulation examples are also provided for these systems comparing the RRT and the RRT* algorithms. Finding asymptotically-optimal paths in multi-robot motion planning problems could be achieved, in principle, using sampling-based planners in the composite configuration space of all of the robots in the space. The dimensionality of this space increases with the number of robots, rendering this approach impractical. This work focuses on a scalable sampling-based planner for coupled multi-robot problems that provides asymptotic optimality. It extends the dRRT approach, which proposed building roadmaps for each robot and searching an implicit roadmap in the composite configuration space. This work presents a new method, dRRT* , and develops theory for scalable convergence to optimal paths in multi-robot problems. Simulated experiments indicate dRRT* converges to high-quality paths while scaling to higher numbers of robots where the naive approach fails. Furthermore, dRRT* is applicable to high-dimensional problems, such as planning for robot manipulators This paper presents a novel randomized motion planner for robots that must achieve a specified goal under kinematic and or dynamic motion constraints while avoiding collision with moving obstacles with known trajectories. The planner encodes the motion constraints on the robot with a control system and samples the robot's state × time space by picking control inputs at random and integrating its equations of motion. The result is a probabilistic roadmap of sampled state ×time points, called milestones, connected by short admissible trajectories. The planner does not precompute the roadmap; instead, for each planning query, it generates a new roadmap to connect an initial and a goal state×time point. The paper presents a detailed analysis of the planner's convergence rate. It shows that, if the state×time space satisfies a geometric property called expansiveness, then a slightly idealized version of our implemented planner is guaranteed to find a trajectory when one exists, with probability quickly converg... A new framework which adopts a rapidly-exploring random tree (RRT) path planner with a Gaussian process (GP) occupancy map is developed for the navigation and exploration of an unknown but cluttered environment. The GP map outputs the probability of occupancy given any selected query point in the continuous space and thus makes it possible to explore the full space when used in conjunction with a continuous path planner. Furthermore, the GP map-generated path is embedded with the probability of collision along the path which lends itself to obstacle avoidance. Finally, the GP map-building algorithm is extended to include an exploration mission considering the differential constraints of a rotary unmanned aerial vehicle and the limitation arising from the environment. Using mutual information as an information-theoretic measure, an informative path which reduces the uncertainty of the environment is generated. Simulation results show that GP map combined with RRT planner can achieve the 3D navigation and e... We present a new approach to path planning for deformable linear (one-dimensional) objects such as flexible wires. We introduce a method for efficiently computing stable configurations of a wire subject to manipulation constraints. These configurations correspond to minimal-energy curves. By restricting the planner to minimal-energy curves, the execution of a path becomes easier. Our curve representation is adaptive in the sense that the number of parameters automatically varies with the complexity of the underlying curve. We introduce a planner that computes paths from one minimal-energy curve to another such that all intermediate curves are also minimal-energy curves. This planner can be used as a powerful local planner in a sampling-based roadmap method. This makes it possible to compute a roadmap of the entire "shape space," which is not possible with previous approaches. Using a simplified model for obstacles, we can find minimal-energy curves of fixed length that pass through specified tangents at given control points. Our work has applications in cable routing, and motion planning for surgical suturing and snake-like robots We present Kinodynamic RRT*, an incremental sampling-based approach for asymptotically optimal motion planning for robots with linear dynamics. Our approach extends RRT*, which was introduced for holonomic robots [10], by using a fixed-final-state-free-final-time controller that optimally connects any pair of states, where the cost function is expressed as a trade-off between the duration of a trajectory and the expended control effort. Our approach generalizes earlier work on RRT* for kinodynamic systems, as it guarantees asymptotic optimality for any system with controllable linear dynamics, in state spaces of any dimension. In addition, we show that for the rich subclass of systems with a nilpotent dynamics matrix, closed-form solutions for optimal trajectories can be derived, which keeps the computational overhead of our algorithm compared to traditional RRT* at a minimum. We demonstrate the potential of our approach by computing asymptotically optimal trajectories in three challenging motion planning scenarios: (i) a planar robot with a 4-D state space and double integrator dynamics, (ii) an aerial vehicle with a 10-D state space and linearized quadrotor dynamics, and (iii) a car-like robot with a 5-D state space and non-linear dynamics. Some recent works in conditional planning have proposed reachability heuristics to improve planner scalability, but many lack a formal description of the properties of their distance estimates. To place previous work in context and extend work on heuristics for conditional planning, we provide a formal basis for distance estimates between belief states. We give a definition for the distance between belief states that relies on aggregating underlying state distance measures. We give several techniques to aggregate state distances and their associated properties. Many existing heuristics exhibit a subset of the properties, but in order to provide a standardized comparison we present several generalizations of planning graph heuristics that are used in a single planner. We compliment our belief state distance estimate framework by also investigating efficient planning graph data structures that incorporate BDDs to compute the most effective heuristics. We developed two planners to serve as test-beds for our investigation. The first, CAltAlt, is a conformant regression planner that uses A* search. The second, POND, is a conditional progression planner that uses AO* search. We show the relative effectiveness of our heuristic techniques within these planners. We also compare the performance of these planners with several state of the art approaches in conditional planning. We present a new retraction algorithm for high DOF articulated models and use our algorithm to improve the performance of RRT planners in narrow passages. The retraction step is formulated as a constrained optimization problem and performs iterative refinement on the boundary of C-Obstacle space. We also combine the retraction algorithm with decomposition planners to handle very high DOF articulated models. The performance of our approach is analyzed using Voronoi diagrams and we show that our retraction algorithm provides a good approximation to the ideal RRT-extension in constrained environments. We have implemented our algorithm and tested its performance on robots with more than 40 DOFs in complex environments. In practice, we observe significant performance (2–80X) improvement over prior RRT planners on challenging scenarios with narrow passages. We present an approach for asymptotically optimal motion planning for kinodynamic systems with arbitrary nonlinear dynamics amid obstacles. Optimal sampling-based planners like RRT*, FMT*, and BIT* when applied to kinodynamic systems require solving a two-point boundary value problem (BVP) to perform exact connections between nodes in the tree. Two-point BVPs are non-trivial to solve, hence the prevalence of alternative approaches that focus on specific instances of kinodynamic systems, use approximate solutions to the two-point BVP, or use random propagation of controls. In this work, we explore the feasibility of exploiting recent advances in numerical optimal control and optimization to solve these two-point BVPs for arbitrary kinodynamic systems and how they can be integrated with existing optimal planning algorithms. We combine BIT* with a two-point BVP solver that uses sequential quadratic programming (SQP). We consider the problem of computing minimum-time trajectories. Since the duration of trajectories is not known a-priori, we include the time-step as part of the optimization to allow SQP to optimize over the duration of the trajectory while keeping the number of discrete steps fixed for every connection attempted. Our experiments indicate that using a two-point BVP solver in the inner-loop of BIT* is competitive with the state-of-the-art in sampling-based optimal planning that explicitly avoids the use of two-point BVP solvers. Kinodynamic RRT planners are considered to be general tools for effectively finding feasible trajectories for high-dimensional dynamical systems. However, they struggle when holonomic constraints are present in the system, such as those arising in parallel manipulators, in robots that cooperate to fulfill a given task, or in situations involving contacts with the environment. In such cases, the state space becomes an implicitly-defined manifold, which makes the diffusion heuristic inefficient and leads to inaccurate dynamical simulations. To address these issues, this paper presents an extension of the kinodynamic RRT planner that constructs an atlas of the state-space manifold incrementally, and uses this atlas both to generate random states and to dynamically steer the system towards such states. To the best of our knowledge, this is the first randomized kinodynamic planner that explicitly takes holonomic constraints into account. We validate the approach in significantly-complex systems. In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm FMT*. The algorithm is specifically aimed at solving complex motion planning problems in high-dimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM* and RRT*. The FMT* algorithm performs a 'lazy' dynamic programming recursion on a predetermined number of probabilistically drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space. As such, this algorithm combines features of both single-query algorithms chiefly RRT and multiple-query algorithms chiefly PRM, and is reminiscent of the Fast Marching Method for the solution of Eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the FMT* algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for convergence rate bounds-the first in the field of optimal sampling-based motion planning. Specifically, for a certain selection of tuning parameters and configuration spaces, we obtain a convergence rate bound of order On −1 d+? , where n is the number of sampled points, d is the dimension of the configuration space, and ? is an arbitrarily small constant. We go on to demonstrate asymptotic optimality for a number of variations on FMT*, namely when the configuration space is sampled non-uniformly, when the cost is not arc length, and when connections are made based on the number of nearest neighbors instead of a fixed connection radius. Numerical experiments over a range of dimensions and obstacle configurations confirm our theoretical and heuristic arguments by showing that FMT*, for a given execution time, returns substantially better solutions than either PRM* or RRT*, especially in high-dimensional configuration spaces and in scenarios where collision-checking is expensive. We present an experience-based planning framework called Thunder that learns to reduce computation time required to solve high-dimensional planning problems in varying environments. The approach is especially suited for large configuration spaces that include many invariant constraints, such as those found with whole body humanoid motion planning. Experiences are generated using probabilistic sampling and stored in a sparse roadmap spanner (SPARS), which provides asymptotically near-optimal coverage of the configuration space, making storing, retrieving, and repairing past experiences very efficient with respect to memory and time. The Thunder framework improves upon past experience-based planners by storing experiences in a graph rather than in individual paths, eliminating redundant information, providing more opportunities for path reuse, and providing a theoretical limit to the size of the experience graph. These properties also lead to improved handling of dynamically changing environments, reasoning about optimal paths, and reducing query resolution time. The approach is demonstrated on a 30 degrees of freedom humanoid robot and compared with the Lightning framework, an experience-based planner that uses individual paths to store past experiences. In environments with variable obstacles and stability constraints, experiments show that Thunder is on average an order of magnitude faster than Lightning and planning from scratch. Thunder also uses 98.8 less memory to store its experiences after 10,000 trials when compared to Lightning. Our framework is implemented and freely available in the Open Motion Planning Library. 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, e.g., in terms of a given cost function, have been established so far. The purpose of this paper is to fill this gap, by designing efficient incremental samplingbased algorithms with provable optimality properties. The first contribution of this paper is a negative result: it is proven that, under mild technical conditions, the cost of the best path returned by RRT converges almost surely to a non-optimal value, as the number of samples increases. Second, a new algorithm is considered, called the Rapidly-exploring Random Graph (RRG), and it is shown that the cost of the best path returned by RRG converges to the optimum almost surely. Third, a tree version of RRG is introduced, called RRT∗, 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.