Efficient computation of optimal navigation functions for nonholonomic planning

We present a fast, numerical approach to computing optimal feedback motion strategies for a nonholonomic robot in a cluttered environment. Although many techniques exist to compute navigation functions that can incorporate feedback, none of these methods is directly able to determine optimal strategies for general nonholonomic systems. Our approach builds on previous techniques in numerical optimal control, and on our previous efforts in developing algorithms that compute feedback strategies for problems that involve nondeterministic and stochastic uncertainties in prediction. The proposed approach efficiently computes an optimal navigation function for nonholonomic systems by exploiting two ideas: 1) the principle of Dijkstra´s algorithm can be generalized to continuous configuration spaces and nonholonomic systems; and 2) a simplicial mesh representation can be used to reduce the complexity of numerical interpolation