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
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.