Discrete geometric motion control of autonomous vehicles

The goal of this work is to develop methods to optimally control autonomous robotic vehicles in natural environments. The main contribution is the derivation of state-space structure respecting integration and optimization schemes for mechanical systems with symmetries, controllable shape dynamics, and nonholonomic constraints based on the theory of discrete mechanics. At the core of this approach lies the discretization of variational principles of mechanics that results in various numerical benefits previously unexplored in the area. The resulting framework is then used as a basis for developing optimal control methods applicable to various systems. Developed examples include simplified models of a car, a helicopter, a snakeboard, and a boat. The resulting algorithms are numerically stable, preserve the mechanical geometric structure, and are numerically competitive to existing methods. In addition, two important extensions with view towards practical applications are proposed. First, complex constraints are handled more robustly using homotopy continuation---the process of relaxing nontrivial motion constraints arising either from complicated dynamics or from obstacles in the environment and then smoothly transforming the solution of such easier problem into the original one by deforming the constraints back to their original shape. Second, the optimality and computational efficiency of solution trajectories is addressed by combining discrete mechanics and optimal control (DMOC) with sampling-based roadmaps---a motion planning method focused on global exploration of the state-space. This allows the composition of simple locally optimal DMOC solution trajectories into near globally optimal motions that can handle complex, cluttered environments.