Model Predictive Static Programming for Discrete-Time Optimal Control on Lie Groups
This paper extends the Model Predictive Static Programming (MPSP) framework for nonlinear systems evolving on Euclidean spaces to simple mechanical systems evolving on Lie groups. Classical optimal control approaches based on Pontryagin's Maximum Principle (PMP) lead to nonlinear two-point boundary value problems (TPBVPs), whose numerical solution becomes particularly challenging on nonlinear configuration spaces. To overcome this difficulty, the proposed Lie-group MPSP framework reformulates the finite-horizon optimal control problem as a sequence of static quadratic programs that admit closed-form control updates, thereby avoiding the need to solve TPBVPs directly. The development relies on left-trivialized variations, intrinsic linearization on Lie groups, and a recursive computation of terminal sensitivity matrices, which together enable computationally efficient real-time implementation. The proposed method is demonstrated through optimal flipping maneuvers of a variable-pitch quadrotor (VPQ) and a single-main-rotor helicopter (SMRH), both of which are capable of generating negative thrust. For validation, continuous-time necessary and sufficient optimality conditions are derived, and the corresponding TPBVP solutions are compared against the trajectories generated by the proposed MPSP method in numerical simulations. In addition, the proposed algorithm is systematically compared with the iterative Linear Quadratic Regulator (iLQR) method, and a detailed numerical study is presented to highlight the relative performance and computational features of the two approaches.