Fast Sample-Based Planning for Dynamic Systems by Zero-Control Linearization-Based Steering

Abstract

We propose linearizing about zero-control trajectories of the dynamics and cost instead of linearizing about a single point for steering and distance computations in RRT-like motion planning for highly dynamic systems. Formulated as a time-varying Linear Quadratic Regulator problem, the proposed steering is designed to be efficient and numerically tractable. We describe the computational trade-offs that arise when compared to solving a conventional time-invariant LQR, and provide numerical results for a 3-link inverted pendulum on a cart for a wide range of look-aheads (from hundredths of a second to a second). We find that planning with longer time horizons for the cart-pendulum system requires fewer total vertices, leading to faster exploration than short look-aheads as are customary when linearizing around a single state depending on the density of the obstacles.