A quadratic regulator-based heuristic for rapidly exploring state space

Abstract

Kinodynamic planning algorithms like Rapidly-Exploring Randomized Trees (RRTs) hold the promise of finding feasible trajectories for rich dynamical systems with complex, non-convex 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 proximity 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. Here we argue that the RRTs' coverage of state space is maximized by using a proximity psuedometric proportional to the length, in time, of the quickest possible trajectory between two points in state space. We derive this minimum-time metric for the double integrator and show that an affine quadratic regulator (AQR) design can be used to approximate the exact minimum-time proximity 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. However, for more complex nonlinear systems, experiments thus far suggest that the AQR-based proximity pseudometric and the conventional metric produce equivalent coverage of the state space, on average. This drop-off in benefit as system complexity and nonlinearity increase may be due to the linearization of system dynamics that is required to calculate the AQR-based pseudometric. Future work includes exploring methods for approximating the exact minimum-time proximity pseudometric that can reason about dynamics with higher-order terms.