On-Manifold Model Predictive Control for Trajectory Tracking on Robotic Systems

Robotic systems usually evolve on manifolds, which are often overparameterized or minimally parameterized (but with singularities) in model predictive control (MPC). How to naturally incorporate the system manifold constraints without overparameterization or singularity is a fundamental problem when deploying MPC on robotic systems. In this article, we propose a unified on-manifold MPC framework to address this issue. This framework first formulates the MPC based on a canonical representation of on-manifold systems. Then, the on-manifold MPC is solved by linearizing the system at each point along the trajectory under tracking. There are two main advantages of the proposed scheme. First, the linearized system leads to an equivalent error system represented by local coordinates without singularities. Second, the process of system modeling, error-system derivation, linearization, and control has the manifold constraints completely decoupled from the system descriptions, enabling us to develop a symbolic MPC framework naturally encapsulating the manifold constraints. In this framework, one needs only to supply system-specific descriptions without dealing with the manifold constraints. To validate the generality of the proposed framework, we implement it on two different robotic platforms, a quadrotor unmanned aerial vehicle (UAV) evolving on a Lie group, and an unmanned ground vehicle moving on curved surface with a non-Lie group structure. To validate the nonsingularity and tracking performance, we test it in tracking aggressive UAV trajectories. Experimental results show that with a single, global on-manifold MPC, the quadrotor tracks highly aggressive trajectories with large actuator efforts and attitude variation ( $360^\circ$ ) in both roll and pitch directions.