LQ-OCP: Energy-Optimal Control for LQ Problems

This article presents a method to automatically generate energy-optimal trajectories for systems with linear dynamics, linear constraints, and a quadratic cost functional (LQ systems). First, using recent advancements in optimal control, we derive the optimal motion primitive generator for LQ systems--this yields linear differential equations that describe all dynamical motion primitives that the optimal system follows. We also derive the optimality conditions where the system switches between motion primitives--a system of equations that are bilinear in the unknown junction time. Finally, we demonstrate the performance of our approach on an energy-minimizing submersible robot with state and control constraints. We compare our approach to an energy-optimizing Linear Quadratic Regulator (LQR), where we learn the optimal weights of the LQR cost function to minimize energy consumption while ensuring convergence and constraint satisfaction. Our approach converges to the optimal solution 6,400% faster than the LQR weight optimization, and that our solution is 350% more energy efficient. Finally, we disturb the initial state of the submersible to show that our approach still finds energy-efficient solutions faster than LQR when the unconstrained solution is infeasible.