MPC basics

 an MPC (Model Predictive Control) problem formulation, where the goal is to drive the center of mass (CoM) of a robot to a desired reference position while ensuring it stays within a support polygon and satisfies various constraints (such as position, velocity, and acceleration). Let's break down the terms and the optimization problem.

Objective Function:

The optimization problem minimizes an objective function that includes:

1. State tracking error:

   • $\sum_{k=0}^{N-1} (x_k - r_k)^T Q (x_k - r_k)$: This term minimizes the difference between the current state $x_k$ and the reference $r_k$, weighted by the matrix $Q$.
   • $(x_N - r_N)^T Q_N (x_N - r_N)$: Similar to the previous term but for the final state at $k = N$, with a different weighting matrix $Q_N$.

2. Control effort:

   • $\sum_{k=0}^{N-1} u_k^T R u_k$: This term minimizes the control effort (the control inputs $u_k$ at each time step $k$), weighted by the matrix $R$.

3. Slack variables (epsilon terms):

   • $\sum_{k=1}^{N} \epsilon_k^T P \epsilon_k$: This term adds penalties for slack variables $\epsilon_k$, which are used to handle possible constraint violations. The matrix $P$ determines the weight of the penalty for violating constraints.

Constraints:

The problem is subject to several constraints:

1. State dynamics:

   • $x_{k+1} = A x_k + B u_k$ (Equation 2b): This constraint represents the system dynamics, where $x_k$ is the state at time step $k$, $A$ is the system matrix, $B$ is the input matrix, and $u_k$ is the control input at time step $k$.

2. State constraints:

   • $F x_k \leq f$ (Equation 2c): These are constraints on the state $x_k$ (position, velocity, etc.). The matrix $F$ and vector $f$ define upper and lower bounds on the state variables at each time step.

3. Input constraints:

   • $M u_k \leq m$ (Equation 2d): These are constraints on the control inputs $u_k$ (e.g., motor torques or forces). The matrix $M$ and vector $m$ define the allowable limits for the control inputs.

4. Support polygon and slack variable constraints:

   • $S_k x_k \leq s_k + \epsilon_k$ (Equation 2e): These constraints ensure that the CoM stays within the support polygon. The matrix $S_k$ defines the boundary of the support polygon, and $\epsilon_k$ is a slack variable that allows some flexibility if necessary.

5. Non-negativity of slack variables:

   • $\epsilon_k \geq 0$ (Equation 2f): This constraint ensures that the slack variables $\epsilon_k$ are non-negative, meaning that any violation of constraints is penalized and cannot be negative.