Discrete Geometric Optimal Control on Lie Groups

We consider the optimal control of mechanical systems on Lie groups and develop numerical methods that exploit the structure of the state space and preserve the system motion invariants. Our approach is based on a coordinate-free variational discretization of the dynamics that leads to structure-preserving discrete equations of motion. We construct necessary conditions for optimal trajectories that correspond to discrete geodesics of a higher order system and develop numerical methods for their computation. The resulting algorithms are simple to implement and converge to a solution in very few iterations. A general software implementation is provided and applied to two example systems: an underactuated boat and a satellite with thrusters.