We present Kinodynamic RRT*, an incremental sampling-based approach for asymptotically optimal motion planning for robots with linear differential constraints. Our approach extends RRT*, which was introduced for holonomic robots [8], by using a fixed-final-state-free-final-time controller that exactly and 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 extending RRT* to kinodynamic sys- tems, as it guarantees asymptotic optimality for any system with controllable linear dynamics, in state spaces of any dimension. Our approach can be applied to non-linear dynamics as well by using their first-order Taylor approximations. 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.

Videos

Video 1. This video shows the development of paths for three different dynamical systems. It then shows the robot following the final path for each of the systems.

Video 2. This video shows the development of a path for a double integrator system. In this case paths belonging to two homotopy classes exist. Our algorithm selects the path from the homotopy class containing the shortest path.

Video 3. This video shows the development of a path for a quadrotor robot. The model used is a linearized model where yaw has been restricted making the model 10-D.

Video 4. This video shows the development of a path for a car-like robot. This model is highly non-linear and the effects of the linearization can be seen around some of the turns.