Trajectory Optimization for Nonholonomic Vehicles on Non-Flat Terrains Using Shooting and Collocation Methods

In this paper we focus on the trajectory optimization problem for a specific family of robots; nonholonomic mobile robots. We study the particular case where such robots operate on smooth, non-flat terrains, i.e. terrains with large differences in elevation. Initially we present the governing equations of such robots and then study the trajectory optimization problem in order to solve for the optimal control policy. We test two different approaches for this problem, namely a shooting and a collocation method, for evaluating and optimizing a performance index.