We present an optimal kinodynamic rapidly exploring random tree, a single query incremental sampling based optimal motion planner for robots with non-linear dynamics, differential constraints and actuation limitations. Our work extends the algorithms presented previously by formulating a fixed-final-state-free-final-time open loop configuration space metric for nearest neighbours search and the appropriate closed loop feedback controller for the tree extension heuristic, allowing us to introduce constraints on actuation magnitude and bandwidth. The controller is formulated by minimizing the amount of energy used to connect two states whereas the trade off is between total trajectory time and weighted actuation norm. We demonstrate the algorithm on (1) a simple 2D pendulum with actuation constraints and (2) a quad rotor 13D model.
展开▼
