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.
Optimal motion planning has been a topic of renewed activity in robotics largely due to the advent of sampling-based motion planners that are proven to be asymptotically optimal @cite_1 . But the community has had a long history of interest in optimal motions. Numerical trajectory optimization techniques @cite_20 @cite_13 @cite_3 @cite_14 have been long studied, but have several drawbacks. First, they are prone to falling into local minima, and second they typically require differentiable constraint and cost representations, which are often hard to produce for complex obstacles. Grid-based planners @cite_6 @cite_5 @cite_9 are often fast in low dimensional spaces but suffer from the curse of dimensionality,'' with performance degrading rapidly in spaces of higher dimension @cite_18 . Sampling-based planners were originally developed to overcome many of these challenges, and have been shown to have excellent empirical performance in finding feasible paths in high-dimensional spaces, both without @cite_17 and with dynamic constraints @cite_22 @cite_25 . However, they tend to produce jerky paths that are far from optimal. Some hybrid approaches have combined sampling-based planning with local optimization to produce better paths @cite_4 @cite_0 .
@cite_18 @cite_14 @cite_4 @cite_22 @cite_9 @cite_1 @cite_3 @cite_6 @cite_0 @cite_5 @cite_13 @cite_25 @cite_20 @cite_17
2729017031 2024382047 2099893201 2613401634 2036016432 1485487406 1968478252 1974733785 2143088799 2128990851 1983885373 2412669390 1986004895 2045726379 2963439114 2007193228 2569270512 2101888913 61873113 2041006327
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 As sampling-based motion planners become faster, they can be reexecuted more frequently by a robot during task execution to react to uncertainty in robot motion, obstacle motion, sensing noise, and uncertainty in the robot's kinematic model. We investigate and analyze high-frequency replanning (HFR) where, during each period, fast sampling-based motion planners are executed in parallel as the robot simultaneously executes the first action of the best motion plan from the previous period. We consider discrete-time systems with stochastic nonlinear (but linearizable) dynamics and observation models with noise drawn from zero mean Gaussian distributions. The objective is to maximize the probability of success (i.e., avoid collision with obstacles and reach the goal) or to minimize path length subject to a lower bound on the probability of success. We show that, as parallel computation power increases, HFR offers asymptotic optimality for these objectives during each period for goal-oriented problems. We then demonstrate the effectiveness of HFR for holonomic and nonholonomic robots including car-like vehicles and steerable medical needles. Existing high-dimensional motion planning algorithms are simultaneously overpowered and underpowered. In domains sparsely populated by obstacles, the heuristics used by sampling-based planners to navigate “narrow passages” can be needlessly complex; furthermore, additional post-processing is required to remove the jerky or extraneous motions from the paths that such planners generate. In this paper, we present CHOMP, a novel method for continuous path refinement that uses covariant gradient techniques to improve the quality of sampled trajectories. Our optimization technique both optimizes higher-order dynamics and is able to converge over a wider range of input paths relative to previous path optimization strategies. In particular, we relax the collision-free feasibility prerequisite on input paths required by those strategies. As a result, CHOMP can be used as a standalone motion planner in many real-world planning queries. We demonstrate the effectiveness of our proposed method in manipulation planning for a 6-DOF robotic arm as well as in trajectory generation for a walking quadruped robot. We study the sparsity and optimality properties of crowd navigation and find that existing techniques do not satisfy both criteria simultaneously: either they achieve optimality with a prohibitive number of samples or tractability assumptions make them fragile to catastrophe. For example, if the human and robot are modeled independently, then tractability is attained but the planner is prone to overcautious or overaggressive behavior. For sampling based motion planning of joint human-robot cost functions, for @math agents and @math step lookahead, @math samples are needed for coverage of the action space. Advanced approaches statically partition the action space into free-space and then sample in those convex regions. However, if the human is into free-space, then the partition is misleading and sampling is unsafe: free space will soon be occupied. We diagnose the cause of these deficiencies---optimization happens over space---and propose a novel solution: optimize over trajectory space by using a Gaussian process (GP) basis. We exploit the "kernel trick" of GPs, where a continuum of trajectories are captured with a mean and covariance function. By using the mean and covariance as proxies for a trajectory family we reason about collective trajectory behavior without resorting to sampling. The GP basis is sparse and optimal with respect to collision avoidance and robot and crowd intention and flexibility. GP sparsity leans heavily on the insight that joint action space decomposes into free regions; however, the decomposition contains feasible solutions only if the partition is dynamically generated. We call our approach . 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... 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. In this paper, a new method is presented for motion planning in dynamic environments, that is, finding a trajectory for a robot in a scene consisting of both static and dynamic, moving obstacles. We propose a practical algorithm based on a roadmap that is created for the static part of the scene. On this roadmap, an approximately time-optimal trajectory from a start to a goal configuration is computed, such that the robot does not collide with any moving obstacle. The trajectory is found by performing a two-level search for a shortest path. On the local level, trajectories on single edges of the roadmap are found using a depth-first search on an implicit grid in state-time space. On the global level, these local trajectories are coordinated using an A sup * -search to find a global trajectory to the goal configuration. The approach is applicable to any robot type in configuration spaces with any dimension, and the motions of the dynamic obstacles are unconstrained, as long as they are known beforehand. The approach has been implemented for both free-flying and articulated robots in three-dimensional workspaces, and it has been applied to multirobot motion planning, as well. Experiments show that the method achieves interactive performance in complex environments. Randomized planners, search-based planners, potential-field approaches and trajectory optimization based motion planners are just some of the types of approaches that have been developed for motion planning. Given a motion planning problem, choosing the appropriate algorithm to use is a daunting task even for experts since there has been relatively little effort in comparing the plans generated by the different approaches, for different problems. In this paper, we present a set of benchmarks and the associated infrastructure for comparing different types of motion planning approaches and algorithms. The benchmarks are specifically designed for robotics and include typical indoor human environments. We present example motion planning problems for single arm tasks. Our infrastructure is designed to be easily extensible to allow for the addition of new planning approaches, new robots, new environments and new metrics. We present results comparing the performance of several motion planning algorithms to validate the use of these benchmarks. We propose three sampling-based motion planning algorithms for generating informative mobile robot trajectories. The goal is to find a trajectory that maximizes an information quality metric (e.g. variance reduction, information gain, or mutual information) and also falls within a pre-specified budget constraint (e.g. fuel, energy, or time). Prior algorithms have employed combinatorial optimization techniques to solve these problems, but existing techniques are typically restricted to discrete domains and often scale poorly in the size of the problem. Our proposed rapidly exploring information gathering (RIG) algorithms combine ideas from sampling-based motion planning with branch and bound techniques to achieve efficient information gathering in continuous space with motion constraints. We provide analysis of the asymptotic optimality of our algorithms, and we present several conservative pruning strategies for modular, submodular, and time-varying information objectives. We demonstrate that our proposed techniques find optimal solutions more quickly than existing combinatorial solvers, and we provide a proof-of-concept field implementation on an autonomous surface vehicle performing a wireless signal strength monitoring task in a lake. A new motion planning method for robots in static workspaces is presented. This method proceeds in two phases: a learning phase and a query phase. In the learning phase, a probabilistic roadmap is constructed and stored as a graph whose nodes correspond to collision-free configurations and whose edges correspond to feasible paths between these configurations. These paths are computed using a simple and fast local planner. In the query phase, any given start and goal configurations of the robot are connected to two nodes of the roadmap; the roadmap is then searched for a path joining these two nodes. The method is general and easy to implement. It can be applied to virtually any type of holonomic robot. It requires selecting certain parameters (e.g., the duration of the learning phase) whose values depend on the scene, that is the robot and its workspace. But these values turn out to be relatively easy to choose, Increased efficiency can also be achieved by tailoring some components of the method (e.g., the local planner) to the considered robots. In this paper the method is applied to planar articulated robots with many degrees of freedom. Experimental results show that path planning can be done in a fraction of a second on a contemporary workstation ( spl ap 150 MIPS), after learning for relatively short periods of time (a few dozen seconds). The approach of inferring user's intended task and optimizing low-level robot motions has promise for making robot teleoperation interfaces more intuitive and responsive. But most existing methods assume a finite set of candidate tasks, which limits a robot's functionality. This paper proposes the notion of freeform tasks that encode an infinite number of possible goals (e.g., desired target positions) within a finite set of types (e.g., reach, orient, pick up). It also presents two technical contributions to help make freeform UIs possible. First, an intent predictor estimates the user's desired task, and accepts freeform tasks that include both discrete types and continuous parameters. Second, a cooperative motion planner continuously updates the robot's trajectories to achieve the inferred tasks by repeatedly solving optimal control problems. The planner is designed to respond interactively to changes in the indicated task, avoid collisions in cluttered environments, handle time-varying objective functions, and achieve high-quality motions using a hybrid of numerical and sampling-based techniques. The system is applied to the problem of controlling a 6D robot manipulator using 2D mouse input in the context of two tasks: static target reaching and dynamic trajectory tracking. Simulations suggest that it enables the robot to reach intended targets faster and to track intended trajectories more closely than comparable techniques. With the increased use of high degree-of-freedom robots that must perform tasks in real-time, there is a need for fast algorithms for motion planning. In this work, we view motion planning from a probabilistic perspective. We consider smooth continuous-time trajectories as samples from a Gaussian process (GP) and formulate the planning problem as probabilistic inference. We use factor graphs and numerical optimization to perform inference quickly, and we show how GP interpolation can further increase the speed of the algorithm. Our framework also allows us to incrementally update the solution of the planning problem to contend with changing conditions. We benchmark our algorithm against several recent trajectory optimization algorithms on planning problems in multiple environments. Our evaluation reveals that our approach is several times faster than previous algorithms while retaining robustness. Finally, we demonstrate the incremental version of our algorithm on replanning problems, and show that it often can find successful solutions in a fraction of the time required to replan from scratch. 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. This contribution presents a novel approach for efficient online planning of topologically distinctive mobile robot trajectories. Trajectory optimization deforms an initial coarse path drafted by a global planner with respect to robot motion related objectives and constraints. The primary objective is to reach a goal state in minimal time on a collision free path that adheres to the kinematic and dynamic constraints of the mobile robot. Conventional local planners get often stuck in a local optimal trajectory as they are unable to transit across obstacles. Our approach seeks the globally optimal trajectory as it maintains and optimizes a subset of admissible candidate trajectories of distinctive topologies in parallel. In case of dynamic environments the planner switches to the current globally optimal trajectory among the candidate set. The online trajectory planning with timed elastic bands is tightly integrated with the robot motion feedback control. The comparative analysis with conventional local planners confirms the advantages of maintaining distinctive topologies to circumnavigate dynamic obstacles. A defining feature of sampling-based motion planning is the reliance on an implicit representation of the state space, which is enabled by a set of probing samples. Traditionally, these samples are drawn either probabilistically or deterministically to uniformly cover the state space. Yet, the motion of many robotic systems is often restricted to “small” regions of the state space, due to e.g. differential constraints or collision-avoidance constraints. To accelerate the planning process, it is thus desirable to devise non-uniform sampling strategies that favor sampling in those regions where an optimal solution might lie. This paper proposes a methodology for nonuniform sampling, whereby a sampling distribution is learned from demonstrations, and then used to bias sampling. The sampling distribution is computed through a conditional variational autoencoder, allowing sample generation from the latent space conditioned on the specific planning problem. This methodology is general, can be used in combination with any sampling-based planner, and can effectively exploit the underlying structure of a planning problem while maintaining the theoretical guarantees of sampling-based approaches. Specifically, on several planning problems, the proposed methodology is shown to effectively learn representations for the relevant regions of the state space, resulting in an order of magnitude improvement in terms of success rate and convergence to the optimal cost. In this paper we address the problem of motion planning for multiple robots. We introduce a prioritized method, based on a powerful method for motion planning in dynamic environments, recently developed by the authors. Our approach is generically applicable: there is no limitation on the number of degrees of freedom of each of the robots, and robots of various types - for instance free-flying robots and articulated robots - can be used simultaneously. Results show that high-quality paths can be produced in less than a second of computation time, even in confined environments involving many robots. We examine three issues in particular in this paper: the assignment of priorities to the robots, the performance of prioritized planning versus coordinated planning, and the influence of the extent by which the robot motions are constrained on the performance of the method. Results are reported in terms of both running time and the quality of the paths produced. We present a scalable robot motion planning algorithm for reach-avoid problems. We assume a discrete-time, linear model of the robot dynamics and a workspace described by a set of obstacles and a target region, where both the obstacles and the region are polyhedra. Our goal is to construct a trajectory, and the associated control strategy, that steers the robot from its initial point to the target while avoiding obstacles. Differently from previous approaches, based on the discretization of the continuous state space or uniform discretization of the workspace, our approach, inspired by the lazy satisfiability modulo theory paradigm, decomposes the planning problem into smaller subproblems, which can be efficiently solved using specialized solvers. At each iteration, we use a coarse, obstacle-based discretization of the workspace to obtain candidate high-level, discrete plans that solve a set of Boolean constraints, while completely abstracting the low-level continuous dynamics. The feasibility of the proposed plans is then checked via a convex program, under constraints on both the system dynamics and the control inputs, and new candidate plans are generated until a feasible one is found. To achieve scalability, we show how to generate succinct explanations for the infeasibility of a discrete plan by exploiting a relaxation of the convex program that allows detecting the earliest possible occurrence of an infeasible transition between workspace regions. Simulation results show that our algorithm favorably compares with state-of-the-art techniques and scales well for complex systems, including robot dynamics with up to 50 continuous states. This work deals with trajectory optimization for a robotic sensor network sampling a spatio-temporal random field. We examine the optimal sampling problem of minimizing the maximum predictive variance of the estimator over the space of network trajectories. This is a high-dimensional, multi-modal, nonsmooth optimization problem, known to be NP-hard even for static fields and discrete design spaces. Under an asymptotic regime of near-independence between distinct sample locations, we show that the solutions to a novel generalized disk-covering problem are solutions to the optimal sampling problem. This result effectively transforms the search for the optimal trajectories into a geometric optimization problem. Constrained versions of the latter are also of interest as they can accommodate trajectories that satisfy a maximum velocity restriction on the robots. We characterize the solution for the unconstrained and constrained versions of the geometric optimization problem as generalized multicircumcenter trajectories, and provide algorithms which enable the network to find them in a distributed fashion. Several simulations illustrate our results. We present a novel optimization-based algorithm for motion planning in dynamic environments. Our approach uses a stochastic trajectory optimization framework to avoid collisions and satisfy smoothness and dynamics constraints. Our algorithm does not require a priori knowledge about global motion or trajectories of dynamic obstacles. Rather, we compute a conservative local bound on the position or trajectory of each obstacle over a short time and use the bound to compute a collision-free trajectory for the robot in an incremental manner. Moreover, we interleave planning and execution of the robot in an adaptive manner to balance between the planning horizon and responsiveness to obstacle. We highlight the performance of our planner in a simulated dynamic environment with the 7-DOF PR2 robot arm and dynamic obstacles. Sampling in the space of controls or actions is a well-established method for ensuring feasible local motion plans. However, as mobile robots advance in performance and competence in complex environments, this classical motion-planning technique ceases to be effective. When environmental constraints severely limit the space of acceptable motions or when global motion planning expresses strong preferences, a state space sampling strategy is more effective. Although this has been evident for some time, the practical question is how to achieve it while also satisfying the severe constraints of vehicle dynamic feasibility. The paper presents an effective algorithm for state space sampling utilizing a model-based trajectory generation approach. This method enables high-speed navigation in highly constrained and-or partially known environments such as trails, roadways, and dense off-road obstacle fields. © 2008 Wiley Periodicals, Inc.