Control algorithms convert a desired behavior — a joint angle, a velocity, a trajectory from the path planner — into actuator commands, using feedback from sensors to correct errors and reject disturbances. This page covers the feedback loop, PID control with a runnable simulation, state-space methods (LQR), model predictive control, and how learned controllers compare.
The controller sees the error e(t) = r(t) − y(t) between the reference and the measured output, and computes an input u(t). Closed-loop control tolerates model error and disturbances (a gust, a heavier payload) because it reacts to what actually happens. The state measured in feedback typically comes from the estimator described in Perception & SLAM.
u(t) = Kp e(t) + Ki ∫ e dτ + Kd de/dt. The proportional term reacts to current error, the integral removes steady-state offset (e.g. gravity on an arm), and the derivative damps oscillation by reacting to the error's rate of change. PID needs no model and is still the most common controller in industry, from motor current loops to drone attitude.
import numpy as np
class PID:
def __init__(self, kp, ki, kd, u_min=-10, u_max=10):
self.kp, self.ki, self.kd = kp, ki, kd
self.u_min, self.u_max = u_min, u_max
self.integral, self.prev_y = 0.0, None
def step(self, r, y, dt):
e = r - y
d = 0.0 if self.prev_y is None else -(y - self.prev_y) / dt # derivative on measurement
self.prev_y = y
u = self.kp * e + self.ki * self.integral + self.kd * d
u_sat = np.clip(u, self.u_min, self.u_max)
if u == u_sat: # anti-windup: integrate only if unsaturated
self.integral += e * dt
return u_sat
# Plant: mass on a damped track, m*a = u - c*v
m, c, dt = 1.0, 0.5, 0.01
pos, vel = 0.0, 0.0
pid = PID(kp=8.0, ki=2.0, kd=4.0)
for k in range(1000):
u = pid.step(r=1.0, y=pos, dt=dt)
acc = (u - c * vel) / m
vel += acc * dt; pos += vel * dt
print(f"position after 10 s: {pos:.3f}")Tuning methods include manual tuning (raise Kp until oscillation, add Kd to damp, add Ki to remove offset), Ziegler-Nichols, and model-based tuning. Cascaded loops (position → velocity → current) are standard in motor drives.
With a linear model ẋ = A x + B u, the Linear Quadratic Regulator finds the gain K in u = −K x minimizing ∫ (xT Q x + uT R u) dt. Q penalizes state error and R penalizes control effort, giving an intuitive tuning knob. K = R-1 BT P, where P solves the continuous algebraic Riccati equation.
import numpy as np
from scipy.linalg import solve_continuous_are
# Cart-pole linearized about upright (state: x, x_dot, theta, theta_dot)
g, M, mp, l = 9.81, 1.0, 0.1, 0.5
A = np.array([[0, 1, 0, 0],
[0, 0, -mp * g / M, 0],
[0, 0, 0, 1],
[0, 0, (M + mp) * g / (M * l), 0]])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])
Q = np.diag([1, 1, 10, 1]); R = np.array([[0.1]])
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.solve(R, B.T @ P)
print("LQR gain:", K.round(2))
print("closed-loop eigenvalues:", np.linalg.eigvals(A - B @ K).round(2)) # all negative real parts = stableMPC repeatedly solves a finite-horizon optimal control problem online, applies the first input, and re-plans at the next step. It handles input and state constraints directly and can follow a reference trajectory with preview. Costs: a reasonably accurate model, and a solver that finishes within the control period (fast QP solvers such as OSQP, or acados for nonlinear MPC). It is standard on autonomous vehicles, legged robots and process control.
| Method | Needs model? | Constraints | Typical use |
|---|---|---|---|
| PID | No | Saturation only | Motors, single-axis loops |
| LQR | Linear model | No | Balancing, stabilization near an operating point |
| MPC | Yes | Yes, explicit | Vehicles, legged robots, trajectory tracking |
| RL policy | Simulator | Via reward / shielding | Locomotion, dexterous manipulation |
RL policies (often trained with PPO in simulation) can learn behaviors that are hard to model, but offer weaker guarantees; deployments often wrap them with a classical low-level controller or a safety filter.
On a microcontroller, PID runs at a fixed sample period Ts. The textbook equation becomes u[k] = Kpe[k] + KiTsΣe + Kd(e[k] − e[k−1])/Ts. Four details separate a working controller from one that oscillates or saturates:
Worked example: with Kp = 2, Ki = 1, Kd = 0.1, Ts = 0.01 s, a setpoint of 1.0 and a measurement of 0.8, the error is 0.2. If the previous error was 0.25 and the accumulated integral (before this step) was 0.5, then P = 0.4, I = 1 × (0.5 + 0.2 × 0.01) = 0.502, D = 0.1 × (0.2 − 0.25)/0.01 = −0.5, and u = 0.402. The derivative term is pulling back because the error is shrinking quickly, which is exactly the damping it is meant to provide.
Industrial drives nest loops: the innermost current (torque) loop runs fastest, often at tens of kHz in the drive electronics; a velocity loop wraps it; a position loop wraps that. Each outer loop treats the inner loop as an almost ideal actuator, which only works if the inner loop is several times faster. Tune inside-out: current first, then velocity, then position.
Classical analysis asks how much extra gain or delay the loop tolerates before becoming unstable. Gain margin and phase margin from a Bode plot of the open loop are the standard measures; phase margins of roughly 45–60 degrees are a common design target because they give a well-damped step response. Time delay (communication, computation, sensor filtering) erodes phase margin directly, which is why networked or vision-in-the-loop controllers must be tuned more conservatively.
Controllers such as LQR assume access to the full state, but robots measure only parts of it, with noise. A state estimator fills the gap. For linear systems with Gaussian noise, the Kalman filter is optimal, and by the separation principle the LQR gain and Kalman gain can be designed independently (together, LQG). Nonlinear robots use the extended or unscented Kalman filter, or complementary filters for attitude (fusing gyro integration with accelerometer gravity direction). Poor estimation is a frequent root cause of controller "instability": a lagging velocity estimate behaves like extra delay in the loop.
| Situation | Reasonable choice | Why |
|---|---|---|
| Single motor, unknown model | PID (cascaded) | No model needed, cheap, well understood |
| Balancing robot, quadrotor near hover | LQR on a linearized model | Handles coupled states with one gain matrix |
| Hard limits on states and inputs, preview of the path | MPC | Constraints are part of the optimization |
| Contact-rich manipulation, legged locomotion | MPC, whole-body control, or learned policy with a low-level PD | Hybrid dynamics are hard to model analytically |
| Model only available as a simulator | Reinforcement learning with domain randomization | Learns from simulated experience; needs careful sim-to-real |
Learned policies are commonly deployed as high-level controllers that output joint position targets, with a classical PD loop underneath running at a higher rate. Safety layers (torque limits, joint limits, control barrier functions) stay classical regardless.
Related: Perception and SLAM, Reinforcement Learning.