Week 8 — Control Systems Implementation
Day 3 of 7 | Difficulty: ⭐⭐⭐⭐
Limitations of PID
PID is great for single-joint control. But robots are multi-input, multi-output (MIMO) systems with coupled dynamics. When you move one joint, others feel reaction forces. PID handles each joint independently — it can’t account for coupling.
That’s where state-space control comes in.
State-Space Representation
A robot’s dynamics can be written as:
$$\dot{x} = A x + B u$$
Where:
- $x$ = state vector (positions, velocities, accelerations)
- $u$ = control input (joint torques)
- $A$ = system dynamics matrix
- $B$ = input matrix
For a single joint with position $q$ and velocity $\dot{q}$:
$$x = \begin{bmatrix} q \ \dot{q} \end{bmatrix}, \quad A = \begin{bmatrix} 0 & 1 \ 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \ 1/I \end{bmatrix}$$
Where $I$ = moment of inertia.
Controllability
Before designing a controller, ask: can we actually control this system?
The controllability matrix is:
$$\mathcal{C} = [B \quad AB \quad A^2B \quad \dots \quad A^{n-1}B]$$
If $\text{rank}(\mathcal{C}) = n$ (full rank), the system is controllable — we can drive any state to any other state in finite time.
For robotics: Most well-designed robots are controllable. But underactuated systems (e.g., Acrobot, pendulum on a cart) may not be fully controllable with joint torques alone.
The Linear Quadratic Regulator (LQR)
LQR is the “gold standard” for linear state-feedback control. It finds the optimal gain matrix $K$ that minimizes:
$$J = \int_0^\infty (x^T Q x + u^T R u) dt$$
Where:
- $Q$ = state cost matrix (penalizes deviations from desired state)
- $R$ = control cost matrix (penalizes large control efforts)
Trade-off: Larger $Q$ → tighter tracking, more aggressive control. Larger $R$ → smoother, gentler control.
The LQR Gain
The optimal control law is:
$$u = -K x$$
Where $K$ is computed by solving the Algebraic Riccati Equation (ARE):
$$A^T P + PA - PBR^{-1}B^T P + Q = 0$$
Then $K = R^{-1} B^T P$.
Good news: Python (Control Systems Library, SciPy) solves ARE automatically.
LQR for Robotics: Practical Design
Step 1: Linearize the Dynamics
Robot dynamics are nonlinear: $M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = \tau$.
LQR needs linear dynamics. Linearize around an operating point (e.g., home pose) using Jacobian matrices:
$$A = \frac{\partial f}{\partial x}\bigg|{x_0}, \quad B = \frac{\partial f}{\partial u}\bigg|{x_0}$$
Step 2: Choose Q and R
Q penalizes state error. For joint control:
$$Q = \begin{bmatrix} w_q I & 0 \ 0 & w_v I \end{bmatrix}$$
Where $w_q$ = position weight, $w_v$ = velocity weight. Set $w_q \gg w_v$ if position accuracy matters more.
R penalizes torques:
$$R = w_u I$$
Where $w_u$ = torque cost. Larger $w_u$ → gentler motion.
Step 3: Solve and Implement
import control
import numpy as np
# Define linearized A, B
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1/0.1]]) # I = 0.1 kg·m²
# Weight matrices
Q = np.diag([100, 1]) # Prioritize position accuracy
R = np.array([[0.01]]) # Small torque penalty
# Solve LQR
K, S, E = control.lqr(A, B, Q, R)
print("LQR gain K:", K)
LQR Variants
| Variant | What It Does | When to Use |
|---|---|---|
| LQR | Full state feedback | All states measurable |
| LQG | LQR + Kalman filter | Some states unmeasured |
| MPC | LQR + constraints | Limits on torque, position |
| Infinite-horizon LQR | Steady-state gain | Stationary tasks |
| Finite-horizon LQR | Time-varying gain | Trajectory tracking |
When LQR Beats PID
| Scenario | Winner |
|---|---|
| Single joint, simple task | PID (simpler) |
| Multiple coupled joints | LQR (handles MIMO) |
| Optimal trade-off needed | LQR (minimizes cost) |
| Nonlinear, fast maneuvers | Nonlinear MPC (advanced) |
| Cheap hardware, limited compute | PID (lightweight) |
Key Takeaway
LQR is PID’s big brother: it handles multiple joints simultaneously, optimizes performance, and gives you knobs to trade tracking accuracy against control effort. The cost is complexity — you need a model, a solver, and more CPU.
Next: Day 4 — ROS2 Architecture for Robot Control