ProjekinAja. Robotics Stage 3 · Dynamics Control Lesson 3.9
● Stage 3 · Lesson 3.9

Model Predictive Control: Constraints as the Killer Feature

MPC beats PID when you have hard constraints on states or inputs — without constraints it is usually not worth the computational overhead.

⏱ ~22 min read 📐 Optimal control ⚙ QP solver

🎯 By the end you'll understand

What a receding horizon means and why it outperforms open-loop optimization. How MPC turns each control step into a quadratic program (QP) over a prediction horizon. What state constraints and input constraints do mathematically and physically. When MPC is worth the compute overhead versus when LQR or PID is the right tool. Why infeasibility is the MPC failure mode that demands explicit engineering — and what a safe fallback looks like.

KEY INSIGHTConstraints are not a nuisance — they are the reason MPC exists

LQR is the optimal controller for an unconstrained linear system. MPC is the controller you reach for when the word "unconstrained" is false.

PID and LQR share a hidden assumption: the control signal can be whatever the math demands. In practice, FollowBot's arm joints have torque limits. The mobile base has a maximum velocity. A surgical robot must not exceed a force threshold against tissue. These are hard constraints — violate them and something breaks, someone is hurt, or a regulator shuts you down. The elegant closed-form solution of LQR (u = −Kx) has no mechanism to enforce a constraint of the form |u| ≤ u_max. Model Predictive Control does. That is its killer feature, and understanding it precisely is what separates practitioners who reach for MPC when they should from those who reach for it because it sounds sophisticated.

🔑 The core idea in one sentence

At every control step, MPC solves an optimization problem over a finite future window (the horizon), subject to explicit constraints on states and inputs, takes the first control action from the optimal plan, then discards the rest and re-solves at the next step. The receding window is what makes it feedback control, not open-loop planning.

HOW IT WORKSThe receding horizon: finite planning, infinite correction

Imagine standing on a moving walkway with 10 steps you can see ahead. You plan your weight shifts for those 10 steps to avoid obstacles. You execute step 1. The walkway moves. Now you see a new set of 10 steps — step 2 through 11. You re-plan. This is the receding horizon. Each plan is finite; the control runs indefinitely. Each re-plan incorporates new state information, making the whole thing feedback control despite solving an open-loop optimization at each step.

t k Prediction horizon N = 8 k+N Execute u_k Discard u_{k+1}…u_{k+N-1} k+1 New horizon: k+1 → k+1+N (re-plan with new state measurement) Past → executed steps (measurement closes the feedback loop)
The receding horizon. At step k, the optimizer finds the best sequence u_k…u_{k+N-1}. Only u_k is executed. At k+1 a new measurement arrives, the horizon shifts, and the QP is solved again. The re-planning is what makes MPC feedback — not just prediction.

The horizon length N is a design parameter with two competing pressures. A long horizon captures distant constraints (a velocity wall 8 steps away) and produces smoother behaviour, but increases the size of the QP and thus the solve time. A short horizon is fast but may be "myopic" — accepting an action that is locally optimal but leads into an infeasible region in the near future. In practice, N is chosen so that the QP can be solved reliably within the control period Ts.

💡 Why discard and re-solve instead of executing the full plan?

If the model were perfect, you could compute the optimal sequence once and execute it open-loop. Models are never perfect. Disturbances enter, parameters drift, the prediction model accumulates error. Re-solving at every step with a fresh state measurement closes the loop: the optimization implicitly corrects for model error at each step. This is why MPC is sometimes called "closed-loop MPC" even though the inner optimizer is open-loop. The feedback comes from the measurement, not from the optimizer.

CORE CONCEPTThe QP at each step: cost, model, and constraints

At every control step k, MPC solves the following optimization problem (here for a linear time-invariant system):

📖 The MPC QP (compact form)

minimize    Σi=0N-1 [ xk+iT Q xk+i + uk+iT R uk+i ] + xk+NT P xk+N
subject to: xk+i+1 = A xk+i + B uk+i    (prediction model)
               xmin ≤ xk+i ≤ xmax    (state constraints)
               umin ≤ uk+i ≤ umax    (input constraints)

Let's unpack each piece with physical intuition.

1

Stage cost: Q and R matrices

The term xTQx penalizes deviation of the state from zero (or from a reference). The term uTRu penalizes control effort. These are the same matrices as in LQR (Lesson 3.8), with the same physical interpretation: large Q relative to R means "track tightly, spend energy freely." Large R relative to Q means "be conservative with the actuators, accept more error." Unlike LQR, these weights now operate over a finite horizon — they only matter for the N steps ahead, not forever.

2

Terminal cost: P matrix

The term xk+NTPxk+N penalizes the state at the end of the horizon. Without a terminal cost, a short-horizon MPC can learn to defer cost: it finds solutions that look good for N steps but leave the system in a bad region. A common choice is to set P equal to the LQR infinite-horizon cost matrix (the solution to the Riccati equation). This gives MPC a "look ahead" that implicitly extends beyond the horizon — and it is one reason MPC inherits LQR's stability properties under certain conditions.

3

Prediction model: the equality constraint

The constraint xk+i+1 = Axk+i + Buk+i propagates the state forward through the prediction horizon using the system's linear model. This is why MPC is "model-based" — accuracy of A and B directly determines the accuracy of the predictions. For a nonlinear plant (like a robot arm), A and B are linearized around the current operating point at each step. This is a linearization error that accumulates over the horizon — shorter horizons reduce the error but reduce anticipation. The sim-to-real gap here is the biggest practical headache: if your A matrix is wrong, your predictions are wrong, and your constraints may not actually be satisfied in the real system even when the QP says they are.

4

State constraints: xmin ≤ x ≤ xmax

This is where MPC earns its keep. A joint angle that must not exceed 150° is a state constraint. A robot's position that must stay above the floor is a state constraint. A velocity that must not exceed a legal speed limit is a state constraint. These are linear inequalities over the predicted states, which makes them efficiently expressible as linear constraints in the QP. The optimizer will not return a solution that violates them. This is not a soft suggestion — it is a hard guarantee (within the prediction model's accuracy).

5

Input constraints: umin ≤ u ≤ umax

Actuator limits. A motor that saturates at ±10 N·m, a valve that only opens between 0% and 100%, a wheel that cannot apply more than a physically plausible traction force. Input constraints prevent the optimizer from commanding something the actuator cannot deliver — exactly the failure mode that caused integrator windup in LQR and PID. You can also add a constraint on Δu (the rate of change of the input), which limits how aggressively the controller hammers the actuator.

Once the equality constraints (the model) are substituted in, the QP variables are only the N control inputs uk…uk+N-1. The states become linear functions of those inputs. The resulting QP has N·nu decision variables (where nu is the input dimension) and can be solved efficiently with active-set or interior-point methods. For small systems (a 1D or 2D robot, N = 10–20) this takes microseconds. For larger systems, it can dominate the control budget.

SEE ITMPC constraint demo — 1D double integrator

A 1D double integrator is the simplest nontrivial dynamical system: position x and velocity v, with acceleration a as the input. It is the correct model for a robot driving straight on a flat surface, or for FollowBot's arm joint under simplified conditions. The system is: x[k+1] = x[k] + v[k]·dt; v[k+1] = v[k] + a[k]·dt.

Below, you can set velocity and acceleration (actuation) constraints, then watch MPC respect them while an unconstrained LQR violates them. The QP cost and active constraints are shown at each timestep. The bad decision: make the constraints contradictory — watch the solver declare infeasibility.

⚙ Interactive · MPC vs LQR constraint demo (1D double integrator)

Set constraints with the sliders, then click Run. Observe which trajectories violate the constraints. Trigger infeasibility by narrowing both constraints until they conflict.

1.0
0.8
5
3
MPC position LQR position velocity constraint reference (0 m)
QP at step k=0  ·  horizon N=10  ·  not yet run
⚠ QP INFEASIBLE — constraints are contradictory. No feasible trajectory exists over the horizon. MPC fallback: hold last safe command (u = 0). In production, trigger a safe-stop routine.

Adjust sliders then click Run. Try v_max = 0.2 and a_max = 0.1 together to trigger infeasibility. Note: this widget uses 1-step greedy constraint clipping as a simplified proxy for MPC — it demonstrates constraint enforcement but not true N-step lookahead. A real N=10 QP would anticipate distant constraints; this demo cannot.

The critical observation: LQR ignores your velocity and acceleration limits entirely. It finds the mathematically optimal trajectory and applies whatever control is needed — even if that violates the actuator or state constraints. MPC explicitly plans a trajectory that satisfies all constraints at every predicted step, resulting in a different (slower, smoother, constraint-respecting) trajectory. When the constraints are feasible, MPC is sub-optimal relative to LQR on cost — you pay a performance tax for constraint satisfaction. When the constraints are active (the system would naturally want to exceed them), MPC is the only tool that handles them correctly.

COMPARISONWhen to use MPC — and when not to

MPC is often oversold. Here is an honest accounting.

Situation Right tool Why
Hard actuator limits (torque, velocity, force) MPC LQR/PID have no mechanism to enforce input constraints; anti-windup is a hack, not a guarantee
Hard state constraints (collision avoidance, joint limits) MPC State constraints are not expressible in LQR at all; penalty methods in PID are heuristic
Preview information available (road curvature ahead, known obstacle) MPC MPC naturally incorporates future reference/constraint into the current decision
Simple SISO plant, no constraints, fast loop (e.g., wheel speed control) PID PID is analytically tractable, interpretable, 1 µs to execute; MPC overhead is not justified
Linear time-invariant plant, no constraints, performance matters LQR LQR is globally optimal for the unconstrained LQR problem; MPC is a finite-horizon approximation
Embedded system with <1 ms budget per cycle PID or explicit MPC Online QP is 10–1000× more expensive than PID; explicit MPC pre-computes the solution but only scales to small state dimensions
Highly nonlinear plant (NMPC) Nonlinear MPC (NMPC) Standard MPC linearizes and loses guarantees; NMPC (e.g., CasADi + IPOPT) handles nonlinearity but is far more expensive
💡 MPC is not AI, and it is not magic

MPC is a principled optimization-based feedback controller. It does not learn, it does not generalize, and it is only as good as its model. The performance is bounded by the accuracy of A and B (or the nonlinear model). If your model is wrong by 20%, your constraint guarantees are wrong by at least 20%. Practitioners sometimes confuse "MPC is solving an optimization" with "MPC will find the globally optimal solution." It finds the QP-optimal solution over the horizon, given the model. On the real plant, with model error and disturbances, the achieved performance will differ.

WEAKNESSSolver infeasibility: the failure mode you must engineer for

MPC has a failure mode that PID and LQR do not: the QP may have no feasible solution. This happens when the constraints are mutually contradictory — there is no trajectory that simultaneously satisfies all state and input constraints over the prediction horizon. This is not a software bug; it is a mathematical reality that must be handled at the system level.

⚠ Causes of infeasibility

Contradictory constraints: v_max = 0.1 m/s and a_max = 0.1 m/s² starting from v = 0.05 m/s with a state constraint requiring x to reach a target 5 m away in N steps — there is simply no trajectory that satisfies all three. Tight obstacle clearance: the robot's footprint is modelled as 0.58 m wide, the gap is 0.60 m, and the state constraint enforces 0.05 m clearance — but the gap is only 0.60 m, making 0.05 m clearance impossible. Disturbance-driven infeasibility: a large disturbance pushes the state outside the feasible region; the next QP must find a path back in, but the constraints in the first N steps may make that return impossible.

⚠ Solver timeout is a separate failure

Even when a feasible solution exists, the QP solver may time out if the horizon N is long, the constraint set is large, or the embedded processor is slow. In production systems, the solver is given a wall-clock budget (e.g., 2 ms for a 100 Hz loop). If it exceeds that budget, the result may be suboptimal or absent. A warm-start strategy (initializing the solver with the previous solution shifted by one step) is the standard mitigation — it reduces solve time dramatically on smooth trajectories by providing a good initial feasible point.

🔑 Engineering the fallback

Every MPC deployment must answer the question: "what happens when the QP fails to return a solution in time?" The standard answers, roughly in order of safety: (1) Hold the last safe command — apply the u from the previous step (safe if the plant is slow and the failure is transient). (2) Apply a pre-computed safe fallback — a PID controller or a fixed deceleration command that is guaranteed to satisfy the most critical constraint (e.g., collision avoidance). (3) Emergency stop — set u = 0 or apply maximum braking. The right choice depends on the time constant of the plant. For a robot arm moving at 2 rad/s, "hold last command" for 10 ms is reasonable. For a robot driving at 1.2 m/s toward an obstacle 0.3 m away, it is not.

Constraint relaxation is a common engineering choice for graceful degradation: replace hard constraints with soft constraints by adding a slack variable ε and penalizing it with a large weight M in the cost function (the "big-M" method). The QP is always feasible (ε absorbs any infeasibility), but the large penalty discourages violation. The trade-off: you lose the hard guarantee but gain a controller that always returns a valid control action. This is widely used in automotive MPC where "always returning a command" is more important than "never violating a constraint exactly."

mpc_fallback.pyrclpy · MPC wrapper with fallback
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import numpy as np

class MPCNode(Node):
    def __init__(self):
        super().__init__('mpc_controller')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.last_u = np.array([0.0])   # warm-start and fallback
        self.SOLVER_TIMEOUT_S = 0.008    # 8 ms budget for 100 Hz loop
        self.create_timer(0.01, self.control_cb)

    def control_cb(self):
        state = self.get_current_state()   # x, v from encoder/EKF
        result = self.solve_qp(
            state,
            warm_start=self.last_u,
            timeout=self.SOLVER_TIMEOUT_S
        )

        if result.feasible:
            u = result.u_optimal[0]         # take first step only
            self.last_u = result.u_optimal[1:]  # warm-start for next solve
        else:
            # QP infeasible or timed out — safe fallback
            self.get_logger().warn('MPC QP infeasible — applying fallback')
            u = max(self.last_u[0] - 0.5 * 0.01, 0.0)  # decel at 0.5 m/s²
            self.last_u = np.array([u])

        msg = Twist()
        msg.linear.x = float(u)
        self.pub.publish(msg)

FOLLOWBOTMPC for FollowBot's arm — torque limits and collision clearance

By the end of Stage 3, FollowBot's arm has a derived dynamics model (Newton-Euler from earlier lessons) and a PID velocity controller tuned to a quantitative settling-time spec. MPC appears as the answer to a question PID cannot handle: what happens when the arm is tracking a person's hand near the workspace boundary?

The 6-DOF arm has joint limits (e.g., shoulder abduction ±90°), torque limits (±15 N·m at the shoulder), and a self-collision avoidance constraint (elbow must stay a minimum distance from the mobile base). A PID velocity controller ignores all three. When the tracked person extends their hand to the edge of the workspace, the PID will command the joint to move there — and if the joint limit switch activates, the motion stops abruptly. That abrupt stop is felt as a jerk that can destabilize the arm. With MPC:

1

State: joint angles and velocities

For a single-joint approximation (shoulder pitch, the most constrained joint in person-following), the state is x = [θ, θ̇]T and the input is u = τ (torque). The prediction model linearizes the joint dynamics around the current operating point using the arm's computed inertia matrix M(q) and Coriolis/gravity terms G(q) derived in earlier Stage 3 lessons.

2

Constraints: joint limits and torque limits

Hard constraints: θ ∈ [−1.57, 1.57] rad (±90°), τ ∈ [−15, 15] N·m, θ̇ ∈ [−2.0, 2.0] rad/s. The MPC planner anticipates the joint limit N steps ahead and begins decelerating early — no abrupt stop. The robot extends gracefully to the workspace boundary and holds, rather than hitting the hardware limit switch.

3

Fallback: if QP fails, return to neutral

If the QP is infeasible (e.g., the arm is already at a joint limit and the reference demands it go further), the fallback is a gravity-compensated hold: τ_fallback = G(q). The arm stays where it is without dropping under gravity. This is a physically safe action that does not require a valid QP solution.

🔑 Stage 3 milestone: where MPC fits in the control stack

The final Stage 3 architecture for FollowBot uses layered control: the mobile base uses a PID velocity controller tuned via Ziegler-Nichols (fast, simple, no constraints needed at base speeds), the arm uses MPC for joint-limit and torque-limit aware tracking (constraints are active, overhead is justified), and a feedforward term from inverse dynamics reduces both controllers' error budget. The cross-track error on the figure-eight trajectory is measured on the mobile base (where PID is sufficient) but the arm tracking quality is now measured by joint-limit clearance margin, a metric that LQR/PID cannot even express.

WEAKNESSWhere MPC bites

⚠ Treating the model as ground truth

MPC's constraint guarantees are relative to its prediction model. If the linearized model is 15% off — because the arm is loaded with a payload that wasn't modelled, because joint friction is higher than estimated, or because the discretization step Ts is too coarse — then "constraint satisfied in the QP" does not mean "constraint satisfied in the real world." Practitioners must add a safety margin to each constraint (e.g., use 90% of the true torque limit) to absorb model error. Skipping this step in a first deployment is how robots hit joint stops despite an MPC controller that was supposed to prevent exactly that.

⚠ Horizon too short for the problem's time constant

A horizon of N = 5 steps at Ts = 0.1 s looks 0.5 s ahead. If the arm takes 0.8 s to decelerate from maximum velocity to a joint limit, the MPC sees the constraint only when it is already too late to stop. The minimum viable horizon satisfies N · Ts ≥ (stopping distance) / (max velocity) for all constrained states. For FollowBot's arm at 2.0 rad/s with max deceleration 5 rad/s², the stopping time is 0.4 s — requiring N · Ts ≥ 0.4 s, so at Ts = 0.02 s you need N ≥ 20. Picking N = 10 "because it solved fast" is the single most common MPC design error in student robotics projects.

⚠ Neglecting the infeasibility failure mode in testing

Infeasibility is rarely encountered in simulation because simulation models match MPC's internal model exactly. In the real robot, disturbances, model errors, and sensor noise push the state into regions where the simulator's MPC never went. Teams that only test in simulation sometimes deploy MPC without a fallback controller and discover infeasibility for the first time in the field — at which point the controller returns no command, the robot freezes, and (depending on the platform) gravity or momentum causes an uncontrolled motion. A robust MPC deployment always implements, tests, and deliberately triggers the fallback before any field test.

KEY INSIGHTHow it connects

  • 3.8 · State-Space Representation and LQR — MPC is the constrained generalization of LQR. The Q, R, P matrices carry over directly. LQR is MPC with N → ∞ and no constraints. Understanding why LQR is optimal for the unconstrained case makes the constraint motivation for MPC precise.
  • 4.1 · Probability for Robotics: Belief, Bayes, and Why Point Estimates Fail — MPC uses a point state estimate from the EKF as its initial condition. Stage 4 examines what happens when that initial condition is uncertain, motivating stochastic MPC and tube MPC — extensions that bound constraint violations under state uncertainty. The sim-to-real gap between a certain state (assumed by MPC) and the real uncertain state is the bridge between Stage 3 and Stage 4.

The control stack built across Stage 3 now has three layers: PID for fast, simple loops where constraints are not active; LQR for optimal unconstrained multi-state regulation; and MPC for loops where constraints are hard requirements. Each is the right tool for its domain, and knowing which to reach for — and why — is the practitioner judgment this stage was designed to develop.

GLOSSARYKey terms

Receding horizon control
The MPC strategy of repeatedly solving a finite-horizon optimization problem, executing only the first control action, then advancing the horizon window and re-solving at the next measurement. The receding window is what makes optimization-based control into feedback control.
Quadratic program (QP)
An optimization problem with a quadratic cost function and linear constraints (equalities and inequalities). The form that results when an LQR-like cost is optimized over a finite horizon with linear dynamics and linear state/input constraints. Efficiently solvable with active-set or interior-point algorithms.
State constraint
A hard bound on the system's state variable at any predicted timestep. Examples: joint angle ≤ 90°, velocity ≤ 1.2 m/s, end-effector above floor. Distinguishes MPC from LQR, which has no mechanism to enforce state constraints.
Input constraint
A hard bound on the control signal at any predicted timestep. Examples: torque ≤ 15 N·m, acceleration ∈ [−0.8, 0.8] m/s². Prevents the optimizer from commanding signals the actuator cannot deliver.
QP infeasibility
The condition in which no trajectory exists over the prediction horizon that simultaneously satisfies all state and input constraints. The QP solver returns no solution. Requires an explicit fallback control strategy in any deployed system.
Terminal cost (P matrix)
The weight on the state at the end of the prediction horizon in the MPC cost function. Typically set equal to the LQR infinite-horizon cost-to-go matrix, providing implicit stability and "look-ahead" beyond the finite horizon.
Warm start
Initializing the QP solver for step k+1 using the solution from step k (shifted by one step and padded with a terminal guess). Dramatically reduces solve time on smooth trajectories where the optimal solution changes slowly between steps.
Constraint relaxation (soft constraints)
Replacing hard constraints with soft constraints by introducing slack variables penalized in the cost function. Guarantees that the QP is always feasible but loses the hard satisfaction guarantee. Used when the fallback "always return a command" is more important than "never violate the constraint."

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why does the receding horizon make MPC a feedback controller even though it solves an open-loop optimization at each step?›

The receding horizon incorporates a new state measurement at every control step. Even though the optimization at step k is open-loop (it finds the best sequence of N future inputs given the current state), the fact that the initial condition is re-measured at k+1 before the next solve means the optimization implicitly corrects for disturbances and model error. If the system drifts due to an unmodelled force, the k+1 measurement reflects that drift, and the new QP finds a trajectory from the drifted state. This is feedback: the output (measured state) modifies the next input sequence. Without the measurement update, MPC would be purely open-loop planning.

2FollowBot's arm joint is at θ = 85° (limit is 90°) and MPC is commanding a trajectory that would reach 92° in 3 steps. What should happen?›

The state constraint θ ≤ 90° is active over the prediction horizon. A correctly configured MPC will find a trajectory that stays within [−90°, 90°] — which means decelerating before reaching the limit. The resulting control action will be a lower torque (or even a braking torque) than the unconstrained optimal would produce. If the horizon is long enough and the deceleration is physically achievable within the remaining 5° and available torque, the QP is feasible. If the arm is already moving so fast that it cannot decelerate to stop before 90° within the available torque, the QP may be infeasible — and the fallback (gravity-compensated hold or braking) must be applied. The key test: check whether (θ_current + θ̇_current² / (2 · α_max)) ≤ 90°, where α_max is the maximum deceleration. If not, the system is already kinematically committed to a constraint violation regardless of MPC.

3When is LQR strictly better than MPC, and when is MPC strictly better than LQR?›

LQR is strictly better when: (a) there are no state or input constraints to enforce, (b) the plant is linear time-invariant, (c) the control budget is tight (microseconds per cycle). LQR computes the globally optimal infinite-horizon control law in closed form with a single offline Riccati solve; MPC is a finite-horizon approximation that re-solves a QP online and is never better than LQR on cost for the unconstrained case. MPC is strictly better when: (a) hard input constraints exist (actuator limits that cannot be violated), (b) hard state constraints exist (joint limits, collision avoidance, regulatory force limits), (c) preview information about future references or disturbances can be incorporated. In these cases, LQR has no mechanism to enforce the constraints, so comparison on "cost" is meaningless — LQR may be cheaper but it violates the constraints, making it unacceptable by definition.

4What is constraint relaxation, and when would you use it over hard constraints in MPC?›

Constraint relaxation replaces a hard constraint (e.g., v ≤ v_max) with a soft constraint by introducing a slack variable ε ≥ 0 and penalizing it heavily in the cost: the constraint becomes v ≤ v_max + ε, with +M·ε added to the cost (large M). The QP is now always feasible — ε absorbs any violation, and the large penalty M makes violating it very expensive but not forbidden. Use soft constraints when: (a) the system cannot guarantee feasibility under all conditions (disturbances may push the state outside the reachable set), (b) "always return a valid command" is more important than "never violate the constraint exactly" (most automotive and mobile robot applications), (c) the constraint is a performance requirement rather than a safety-critical physical limit. Do NOT use soft constraints when the constraint is a hard safety limit (collision, hardware damage threshold) where any violation is unacceptable. In those cases, use hard constraints plus a physically safe fallback for the infeasible case.

Next: Stage 4 — probability and belief enter the picture →