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.
Several authors have extended RRT* to dynamically-constrained systems. It is relatively easy to apply RRT* to dynamically-constrained systems if a steering function is available @cite_15 . Proving convergence is harder, requiring analysis of small-time controllability conditions @cite_11 . Other authors have extended RRT* to systems whose dynamics and costs are (or can be approximated) by linear and quadratic functions, respectively, by definition of a suitable steering function based on the LQR principle @cite_8 @cite_7 . When more complex differential constraints are involved, it may not be possible to devise a suitable steering functions. One method generated complex maneuvers using RRT* and performed each rewiring step by numerically solving a two-point boundary value problems @cite_21 . This adds greatly to computational expense. A similar method performed rewiring using a spline-based trajectory representation that is optimized via a nonlinear program solver @cite_19 .
@cite_7 @cite_8 @cite_21 @cite_19 @cite_15 @cite_11
2049561732 1527181952 2071401582 1968651858 2031335381 2054585537 2890926967 1553106265 1969907553 2082784420 1976930960 2887531520 1509540129 2951930445 2335306738 1864112212 2166077797 2161137845 2101340954 2482392012
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. 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. We discuss an implementation of the RRT* optimal motion planning algorithm for the half-car dynamical model to enable autonomous high-speed driving. To develop fast solutions of the associated local steering problem, we observe that the motion of a special point (namely, the front center of oscillation) can be modeled as a double integrator augmented with fictitious inputs. We first map the constraints on tire friction forces to constraints on these augmented inputs, which provides instantaneous, state-dependent bounds on the curvature of geometric paths feasibly traversable by the front center of oscillation. Next, we map the vehicle's actual inputs to the augmented inputs. The local steering problem for the half-car dynamical model can then be transformed to a simpler steering problem for the front center of oscillation, which we solve efficiently by first constructing a curvature-bounded geometric path and then imposing a suitable speed profile on this geometric path. Finally, we demonstrate the efficacy of the proposed motion planner via numerical simulation results. 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. 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. We focus on a replanning scenario for quadrotors where considering time efficiency, non-static initial state and dynamical feasibility is of great significance. We propose a real-time B-spline based kinodynamic (RBK) search algorithm, which transforms a position-only shortest path search (such as A * and Dijkstra) into an efficient kinodynamic search, by exploring the properties of B-spline parameterization. The RBK search is greedy and produces a dynamically feasible time-parameterized trajectory efficiently, which facilitates non-static initial state of the quadrotor. To cope with the limitation of the greedy search and the discretization induced by a grid structure, we adopt an elastic optimization (EO) approach as a post-optimization process, to refine the control point placement provided by the RBK search. The EO approach finds the optimal control point placement inside an expanded elastic tube which represents the free space, by solving a Quadratically Constrained Quadratic Programming (QCQP) problem. We design a receding horizon replanner based on the local control property of B-spline. A systematic comparison of our method against two state-of-the-art methods is provided. We integrate our replanning system with a monocular vision-based quadrotor and validate our performance onboard. Abstract. When optimizing the performance of constrained robotic system, the motion trajectory plays a crucial role. In this research the motion planning problem for systems that admit a polynomial description of the system dynamics through differential flatness is tackled by parameterizing the system's so-called flat output as a polynomial spline. Using basic properties of B-splines, sufficient conditions on the spline coefficients are derived ensuring satisfaction of the operating constraints over the entire time horizon. Furthermore, an intuitive relaxation is proposed to tackle conservatism and a supporting software package is released. Finally, to illustrate the overall approach and potential, a numerical benchmark of a flexible link manipulator is discussed. 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. 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 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. In this paper, we present a novel constraint tightening approach for nonlinear robust model predictive control (MPC). This approach uses a simple constructive constraint tightening based on growing tubes. Contrary to other approaches, we require no complex offline computations to obtain a stabilizing control law. Instead, we consider the notion of incremental stabilizability and design tubes based on an estimate of the achievable exponential decay rate. In addition, we show how this tightening can be used as an ad-hoc modification to improve the robustness of MPC without terminal constraints. We study the system theoretic properties of the resulting closed-loop system, including bounds on the region of attraction and the minimal robust positively invariant (RPI) set. Within an MPC framework without terminal constraints, the proposed constraint tightening leads to a nonlinear robust controller without complex design procedures, which makes it appealing for practical applications. 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. Fast and safe navigation of dynamical systems through a priori unknown cluttered environments is vital to many applications of autonomous systems. However, trajectory planning for autonomous systems is computationally intensive, often requiring simplified dynamics that sacrifice safety and dynamic feasibility in order to plan efficiently. Conversely, safe trajectories can be computed using more sophisticated dynamic models, but this is typically too slow to be used for real-time planning. We propose a new algorithm FaSTrack: Fast and Safe Tracking for High Dimensional systems. A path or trajectory planner using simplified dynamics to plan quickly can be incorporated into the FaSTrack framework, which provides a safety controller for the vehicle along with a guaranteed tracking error bound. This bound captures all possible deviations due to high dimensional dynamics and external disturbances. Note that FaSTrack is modular and can be used with most current path or trajectory planners. We demonstrate this framework using a 10D nonlinear quadrotor model tracking a 3D path obtained from an RRT planner. The problem of maneuvering a vehicle through a race course in minimum time requires computation of both longitudinal (brake and throttle) and lateral (steering wheel) control inputs. Unfortunately, solving the resulting nonlinear optimal control problem is typically computationally expensive and infeasible for real-time trajectory planning. This paper presents an iterative algorithm that divides the path generation task into two sequential subproblems that are significantly easier to solve. Given an initial path through the race track, the algorithm runs a forward-backward integration scheme to determine the minimum-time longitudinal speed profile, subject to tire friction constraints. With this fixed speed profile, the algorithm updates the vehicle's path by solving a convex optimization problem that minimizes the resulting path curvature while staying within track boundaries and obeying affine, time-varying vehicle dynamics constraints. This two-step process is repeated iteratively until the predicted lap time no longer improves. While providing no guarantees of convergence or a globally optimal solution, the approach performs very well when validated on the Thunderhill Raceway course in Willows, CA. The predicted lap time converges after four to five iterations, with each iteration over the full 4.5 km race course requiring only thirty seconds of computation time on a laptop computer. The resulting trajectory is experimentally driven at the race circuit with an autonomous Audi TTS test vehicle, and the resulting lap time and racing line is comparable to both a nonlinear gradient descent solution and a trajectory recorded from a professional racecar driver. The experimental results indicate that the proposed method is a viable option for online trajectory planning in the near future. 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. 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. Although the dynamic equations of motion of open-chained robot systems are well known, they are seldom taken into account during the planning of motions. In this work, we show that the dynamics of a robot can be used to produce motions that extend the payload capability beyond the limit set by traditional methods. In particular, we develop a point-to-point weightlifting motion planner for open-chained robots. The governing optimal control problem is converted into a direct, SQP parameter optimization in which the gradient is determined analytically. The joint trajectories are defined by B-spline polynomials along with a time-scale factor. The algorithm is applied to a Puma 762 robot, with its physical limitations incorporated into the formulation. The torque limits are formulated as soft constraints added into the objective function while the position and velocity limits are formulated as hard, linear inequality constraints, on the parameters. The solutions obtained with our algorithm extend the robot's payload capability while reducing the joint torques. Interestingly, nearly all the trajectories found pass through singular configurations, where large internal forces from the robot are applied to the payload and little torque is needed from the motors. Direct methods for trajectory optimization are widely used for planning locally optimal trajectories of robotic systems. Many critical tasks, such as locomotion and manipulation, often involve impacting the ground or objects in the environment. Most state-of-the-art techniques treat the discontinuous dynamics that result from impacts as discrete modes and restrict the search for a complete path to a specified sequence through these modes. Here we present a novel method for trajectory planning of rigid-body systems that contact their environment through inelastic impacts and Coulomb friction. This method eliminates the requirement for a priori mode ordering. Motivated by the formulation of multi-contact dynamics as a Linear Complementarity Problem for forward simulation, the proposed algorithm poses the optimization problem as a Mathematical Program with Complementarity Constraints. We leverage Sequential Quadratic Programming to naturally resolve contact constraint forces while simultaneously optimizing a trajectory that satisfies the complementarity constraints. The method scales well to high-dimensional systems with large numbers of possible modes. We demonstrate the approach on four increasingly complex systems: rotating a pinned object with a finger, simple grasping and manipulation, planar walking with the Spring Flamingo robot, and high-speed bipedal running on the FastRunner platform. We explore the challenges of planning trajectories for quadrotors through cluttered indoor environments. We extend the existing work on polynomial trajectory generation by presenting a method of jointly optimizing polynomial path segments in an unconstrained quadratic program that is numerically stable for high-order polynomials and large numbers of segments, and is easily formulated for efficient sparse computation. We also present a technique for automatically selecting the amount of time allocated to each segment, and hence the quadrotor speeds along the path, as a function of a single parameter determining aggressiveness, subject to actuator constraints. The use of polynomial trajectories, coupled with the differentially flat representation of the quadrotor, eliminates the need for computationally intensive sampling and simulation in the high dimensional state space of the vehicle during motion planning. Our approach generates high-quality trajecrtories much faster than purely sampling-based optimal kinodynamic planning methods, but sacrifices the guarantee of asymptotic convergence to the global optimum that those methods provide. We demonstrate the performance of our algorithm by efficiently generating trajectories through challenging indoor spaces and successfully traversing them at speeds up to 8 m s. A demonstration of our algorithm and flight performance is available at: http: groups.csail.mit.edu rrg quad_polynomial_trajectory_planning.