Solving Optimal Control Problems of Rigid-Body Dynamics with Collisions Using the Hybrid Minimum Principle

Collisions are common in many dynamical systems with real applications. They can be formulated as hybrid dynamical systems with discontinuities automatically triggered when states transverse certain manifolds. We present an algorithm for the optimal control problem of such hybrid dynamical systems based on solving the equations derived from the hybrid minimum principle (HMP). The algorithm is an iterative scheme following the spirit of the method of successive approximations (MSA), and it is robust to undesired collisions observed in the initial guesses. We propose several techniques to address the additional numerical challenges introduced by the presence of discontinuities. The algorithm is tested on disc collision problems whose optimal solutions exhibit one or multiple collisions. Linear convergence in terms of iteration steps and asymptotic first-order accuracy in terms of time discretization are observed when the algorithm is implemented with the forward-Euler scheme. The numerical results demonstrate that the proposed algorithm has better accuracy and convergence than direct methods based on gradient descent. Furthermore, the algorithm is also simpler, more accurate, and more stable than a deep reinforcement learning method.