LAUSR

This letter focuses on the problem of planning robust trajectories for system with initial state uncertainty. While asymptotically-optimal methods have been proposed for many motion planning applications, there is no prior method which is able to guarantee asymptotic (near-)optimality for planning with initial state uncertainty with non-trivial dynamics and no steering function. In this letter, we define a cost function to evaluate state divergence for kinodynamic planning. We prove properties of this function, our system dynamics, and our planners, which allow asymptotically near-optimal planning without a steering function. We then evaluate our two proposed planners, one that uses random restarts, and another that encourages sparsity, in several experiments. Our results suggest that we are able to improve both the trajectory and end state divergence by about half as compared to a previous method, which is not asymptotically near-optimal.