A well-known weakness of the probabilistic path planners is the so-called narrow passage problem, where a region with a relatively low probability of being sampled must be explored to find a solution path. Many strategies have been proposed to alleviate this problem, most of them based on biasing the sampling distribution. When kinematic constraints appear in the problem, the configuration space typically becomes a non-parametrizable, implicit manifold. Unfortunately, this invalidates most of the existing sampling bias approaches, which rely on an explicit parametrization of the space to explore. In this paper, we propose and evaluate three novel strategies to bias the sampling under the presence of narrow passages in kinematically-constrained systems.
The information of the collisions detected while extending an Rapidly-exploring Random Tree (RRT) can be leveraged to bias the sampling @cite_28 . For instance, using this information the dynamic domain of an RRT node can be defined, i.e, the part of the configuration space where it is worth to extend the tree from that node @cite_17 @cite_14 . As we will see, this approach can be adapted to the kinematically-constrained case defining the dynamic domain in the ambient space rather than in the configuration space.
@cite_28 @cite_14 @cite_17
2147213215 2018740177
The situation arising in path planning under kinematic constraints, where the valid configurations define a manifold embedded in the joint ambient space, can be seen as a limit case of the well-known narrow corridor problem. With kinematic constraints, the probability of obtaining a valid configuration by sampling in the joint ambient space is not low but null, which complicates the direct application of sampling-based path planners. This paper presents the AtlasRRT algorithm, which is a planner especially tailored for such constrained systems that builds on recently developed tools for higher-dimensional continuation. These tools provide procedures to define charts that locally parametrize a manifold and to coordinate the charts, forming an atlas that fully covers it. AtlasRRT simultaneously builds an atlas and a bidirectional rapidly exploring random tree (RRT), using the atlas to sample configurations and to grow the branches of the RRTs, and the RRTs to devise directions of expansion for the atlas. The efficiency of AtlasRRT is evaluated in several benchmarks involving high-dimensional manifolds embedded in large ambient spaces. The results show that the combined use of the atlas and the RRTs produces a more rapid exploration of the configuration space manifolds than existing approaches. Kinodynamic planning algorithms like Rapidly-Exploring Randomized Trees (RRTs) hold the promise of finding feasible trajectories for rich dynamical systems with complex, nonconvex constraints. In practice, these algorithms perform very well on configuration space planning, but struggle to grow efficiently in systems with dynamics or differential constraints. This is due in part to the fact that the conventional distance metric, Euclidean distance, does not take into account system dynamics and constraints when identifying which node in the existing tree is capable of producing children closest to a given point in state space. We show that an affine quadratic regulator (AQR) design can be used to approximate the exact minimum-time distance pseudometric at a reasonable computational cost. We demonstrate improved exploration of the state spaces of the double integrator and simple pendulum when using this pseudometric within the RRT framework, but this improvement drops off as systems' nonlinearity and complexity increase. Future work includes exploring methods for approximating the exact minimum-time distance pseudometric that can reason about dynamics with higher-order terms. 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. We present a replanning algorithm for repairing rapidly-exploring random trees when changes are made to the configuration space. Instead of abandoning the current RRT, our algorithm efficiently removes just the newly-invalid parts and maintains the rest. It then grows the resulting tree until a new solution is found. We use this algorithm to create a probabilistic analog to the widely-used D* family of deterministic algorithms, and demonstrate its effectiveness in a multirobot planning domain A simple and efficient randomized algorithm is presented for solving single-query path planning problems in high-dimensional configuration spaces. The method works by incrementally building two rapidly-exploring random trees (RRTs) rooted at the start and the goal configurations. The trees each explore space around them and also advance towards each other through, the use of a simple greedy heuristic. Although originally designed to plan motions for a human arm (modeled as a 7-DOF kinematic chain) for the automatic graphic animation of collision-free grasping and manipulation tasks, the algorithm has been successfully applied to a variety of path planning problems. Computed examples include generating collision-free motions for rigid objects in 2D and 3D, and collision-free manipulation motions for a 6-DOF PUMA arm in a 3D workspace. Some basic theoretical analysis is also presented. The problem of testing complex reactive control systems and validating the effectiveness of multi-agent controllers is addressed. Testing and validation involve searching for conditions that lead to system failure by exploring all adversarial inputs and disturbances for errant trajectories. This problem of testing is related to motion planning. In both cases, there is a goal or specification set consisting of a set of points in state space that is of interest, either for finding a plan, demonstrating failure or for validation. Unlike motion planning problems, the problem of testing generally involves systems that are not controllable with respect to disturbances or adversarial inputs and therefore, the reachable set of states is a small subset of the entire state space. In this work, sampling-based algorithms based on the Rapidly-exploring Random Trees (RRT) algorithm are applied to the testing and validation problem. First, some of the factors that govern the exploration rate of the RRT algorithm are analysed, this analysis serving to motivate some enhancements. Then, three modifications to the original RRT algorithm are proposed, suited for use on uncontrollable systems. First, a new distance function is introduced which incorporates information about the system's dynamics to select nodes for extension. Second, a weighting is introduced to penalize nodes which are repeatedly selected but fail to extend.Third, a scheme for adaptively modifying the sampling probability distribution is proposed, based on tree growth. Application of the algorithm is demonstrated using several examples, and computational statistics are provided to illustrate the effect of each modification. The final algorithm is demonstrated on a 25 state example and results in nearly an order of magnitude reduction in computation time when compared with the traditional RRT. The proposed algorithms are also applicable to motion planning for systems that are not small time locally controllable. Rapidly-exploring Random Trees (RRTs) are effective for a wide range of applications ranging from kinodynamic planning to motion planning under uncertainty. However, RRTs are not as efficient when exploring heterogeneous environments and do not adapt to the space. For example, in difficult areas an expensive RRT growth method might be appropriate, while in open areas inexpensive growth methods should be chosen. In this paper, we present a novel algorithm, Adaptive RRT, that adapts RRT growth to the current exploration area using a two level growth selection mechanism. At the first level, we select groups of expansion methods according to the visibility of the node being expanded. Second, we use a cost-sensitive learning approach to select a sampler from the group of expansion methods chosen. Also, we propose a novel definition of visibility for RRT nodes which can be computed in an online manner and used by Adaptive RRT to select an appropriate expansion method. We present the algorithm and experimental analysis on a broad range of problems showing not only its adaptability, but efficiency gains achieved by adapting exploration methods appropriately. Incremental sampling-based motion planning algorithms such as the Rapidly-exploring Random Trees (RRTs) have been successful in efficiently solving computationally challenging motion planning problems involving complex dynamical systems. A recently proposed algorithm, called the RRT*, also provides asymptotic optimality guarantees, i.e., almost-sure convergence to optimal trajectories (which the RRT algorithm lacked) while maintaining the computational efficiency of the RRT algorithm. In this paper, time-optimal maneuvers for a high-speed off-road vehicle taking tight turns on a loose surface are studied using the RRT* algorithm. Our simulation results show that the aggressive skidding maneuver, usually called the trail-braking maneuver, naturally emerges from the RRT* algorithm as the minimum-time trajectory. Along the way, we extend the RRT* algorithm to handle complex dynamical systems, such as those that are described by nonlinear differential equations and involve high-dimensional state spaces, which may be of independent interest. We also exploit the RRT* as an anytime computation framework for nonlinear optimization problems. The RRT* algorithm has efficiently extended Rapidly-exploring Random Trees (RRTs) to endow it with asymptotic optimality. We propose Goal-Rooted Feedback Motion Trees (GR-FMTs) that honor state input constraints and generate collision-free feedback policies. Given analytic solutions for optimal local steering, GR-FMTs obtain and realize safe, dynamically feasible, and asymptotically optimal trajectories toward goals. Second, for controllable linear systems with linear state input constraints, we propose a fast method for local steering, based on polynomial basis functions and segmentation. GR-FMTs with the method obtain and realize trajectories that are collision-free, dynamically feasible under constraints, and asymptotically optimal within a set we define. The formulation includes linear or quadratic programming of small sizes, where constraints are identified by root-finding in low or medium order of polynomials and added progressively. Recent progress in motion planning has made it possible to determine homotopy inequivalent trajectories between an initial and terminal configuration in a robot configuration space. Current approaches have however either assumed the knowledge of differential one-forms related to a skeletonization of the collision space, or have relied on a simplicial representation of the free space. Both of these approaches are currently however not yet practical for higher dimensional configuration spaces. We propose 2D topological task projections (TTPs): mappings from the configuration space to 2-dimensional spaces where simplicial complex filtrations and persistent homology can identify topological properties of the high-dimensional free configuration space. Our approach only requires the availability of collision free samples to identify winding centers that can be used to determine homotopy inequivalent trajectories. We propose the Winding Augmented RRT and RRT* (WA-RRT RRT*) algorithms using which homotopy inequivalent trajectories can be found. We evaluate our approach in experiments with configuration spaces of planar linkages with 2–10 degrees of freedom. Results indicate that our approach can reliably identify suitable topological task projections and our proposed WA-RRT and WA-RRT* algorithms were able to identify a collection of homotopy inequivalent trajectories in each considered configuration space dimension. Despite the significant advances in path planning methods, highly constrained problems are still challenging. In some situations, the presence of constraints defines a configuration space that is a non-parametrizable manifold embedded in a high-dimensional ambient space. In these cases, the use of sampling-based path planners is cumbersome since samples in the ambient space have low probability to lay on the configuration space manifold. In this paper, we present a new path planning algorithm specially tailored for highly constrained systems. The proposed planner builds on recently developed tools for higher-dimensional continuation, which provide numerical procedures to describe an implicitly defined manifold using a set of local charts. We propose to extend these methods focusing the generation of charts on the path between the two configurations to connect and randomizing the process to find alternative paths in the presence of obstacles. The advantage of this planner comes from the fact that it directly operates into the configuration space and not into the higher-dimensional ambient space, as most of the existing methods do. 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 ( RRT^ X ), the first asymptotically optimal sampling-based motion planning algorithm for real-time navigation in dynamic environments (containing obstacles that unpredictably appear, disappear, and move). Whenever obstacle changes are observed, e.g., by onboard sensors, a graph rewiring cascade quickly updates the search-graph and repairs its shortest-path -to-goal subtree. Both graph and tree are built directly in the robot’s state space, respect the kinematics of the robot, and continue to improve during navigation. ( RRT^ X ) is also competitive in static environments—where it has the same amortized per iteration runtime as RRT and RRT* ( ( n ) ) and is faster than RRT# ( ( ^2 n ) ). In order to achieve (O ( n ) ) iteration time, each node maintains a set of (O ( n ) ) expected neighbors, and the search graph maintains ( )-consistency for a predefined ( ). The Rapidly Exploring Random Tree Star (RRT*) is an extension of the Rapidly Exploring Random Tree path finding algorithm. RRT* guarantees an optimal, collision free path solution but is limited by slow convergence rates and inefficient memory utilization. This paper presents APGD-RRT*, a variant of RRT* which utilizes Artificial Potential Fields to improve RRT* performance, providing relatively better convergence rates. Simulation results under different environments between the proposed APGD-RRT* and RRT* algorithms demonstrate this marked improvement under various test environments. Rapidly-exploring Random Tree star (RRT*) is a recently proposed extension of Rapidly-exploring Random Tree (RRT) algorithm that provides a collision-free, asymptotically optimal path regardless of obstacles geometry in a given environment. However, one of the limitation in the RRT* algorithm is slow convergence to optimal path solution. As a result it consumes high memory as well as time due to the large number of iterations utilised in achieving optimal path solution. To overcome these limitations, we propose the potential function based-RRT* that incorporates the artificial potential field algorithm in RRT*. The proposed algorithm allows a considerable decrease in the number of iterations and thus leads to more efficient memory utilization and an accelerated convergence rate. In order to illustrate the usefulness of the proposed algorithm in terms of space execution and convergence rate, this paper presents rigorous simulation based comparisons between the proposed techniques and RRT* under different environmental conditions. Moreover, both algorithms are also tested and compared under non-holonomic differential constraints. Sampling-based motion planning algorithms, such as the Probabilistic RoadMap (PRM) and the Rapidly-exploring Random Tree (RRT), have received a large and growing amount of attention during the past decade. Most recently, sampling-based algorithms, such as the PRM* and RRT*, that guarantee asymptotic optimality, i.e., almost-sure convergence towards optimal solutions, have been proposed. Despite the experimental success of asymptotically-optimal sampling-based algorithms, their extensions to handle complex non-holonomic dynamical systems remains largely an open problem. In this paper, with the help of results from differential geometry, we extend the RRT* algorithm to handle a large class of non-holonomic dynamical systems. We demonstrate the performance of the algorithm in computational experiments involving the Dubins' car dynamics. 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 an approach to path planning for manipulators that uses Workspace Goal Regions (WGRs) to specify goal end-effector poses. Instead of specifying a discrete set of goals in the manipulator's configuration space, we specify goals more intuitively as volumes in the manipulator's workspace. We show that WGRs provide a common framework for describing goal regions that are useful for grasping and manipulation. We also describe two randomized planning algorithms capable of planning with WGRs. The first is an extension of RRT-JT that interleaves exploration using a Rapidly-exploring Random Tree (RRT) with exploitation using Jacobian-based gradient descent toward WGR samples. The second is the IKBiRRT algorithm, which uses a forward-searching tree rooted at the start and a backward-searching tree that is seeded by WGR samples. We demonstrate both simulation and experimental results for a 7DOF WAM arm with a mobile base performing reaching and pick-and-place tasks. Our results show that planning with WGRs provides an intuitive and powerful method of specifying goals for a variety of tasks without sacrificing efficiency or desirable completeness properties. The RRT* algorithm has recently been proposed as an optimal extension to the standard RRT algorithm [1]. However, like RRT, RRT* is difficult to apply in problems with complicated or underactuated dynamics because it requires the design of a two domain-specific extension heuristics: a distance metric and node extension method. We propose automatically deriving these two heuristics for RRT* by locally linearizing the domain dynamics and applying linear quadratic regulation (LQR). The resulting algorithm, LQR-RRT*, finds optimal plans in domains with complex or underactuated dynamics without requiring domain-specific design choices. We demonstrate its application in domains that are successively torque-limited, underactuated, and in belief space. Rapidly-exploring random trees (RRTs) are pop- ular in motion planning because they find solutions efficiently to single-query problems. Optimal RRTs (RRT*s) extend RRTs to the problem of finding the optimal solution, but in doing so asymptotically find the optimal path from the initial state to every state in the planning domain. This behaviour is not only inefficient but also inconsistent with their single-query nature. For problems seeking to minimize path length, the subset of states that can improve a solution can be described by a prolate hyperspheroid. We show that unless this subset is sam- pled directly, the probability of improving a solution becomes arbitrarily small in large worlds or high state dimensions. In this paper, we present an exact method to focus the search by directly sampling this subset. The advantages of the presented sampling technique are demonstrated with a new algorithm, Informed RRT*. This method retains the same probabilistic guarantees on complete- ness and optimality as RRT* while improving the convergence rate and final solution quality. We present the algorithm as a simple modification to RRT* that could be further extended by more advanced path-planning algorithms. We show exper- imentally that it outperforms RRT* in rate of convergence, final solution cost, and ability to find difficult passages while demonstrating less dependence on the state dimension and range of the planning problem.