Control Theory for Robotics: A Learning Roadmap

Every robot that moves, balances, or tracks a trajectory runs a feedback loop. A motor driver compares a commanded velocity to an encoder reading, computes the error, and corrects the output hundreds of times per second. Control theory is the mathematics behind that loop. This roadmap orders the topics from first principles to model predictive control, with working code and tuning workflows at each stage.

Quick Takeaways

• Learn in five stages: modeling, PID, frequency-domain analysis, state-space and LQR, then MPC.
• Model first. A controller is only as good as the plant model behind it. Identify your BLDC motor or joint dynamics before tuning anything.
• Most industrial joints run PID at 1 kHz inside a cascaded loop (current, velocity, position). Master it before moving on.
• Use ROS2 ros2_control to deploy controllers on real hardware with a fixed real-time update rate.

Stage 1: Modeling the Plant

A plant is the physical system you control. You cannot design a feedback law without a mathematical description of it.

DC Motor Transfer Function

For a brushed DC motor (a good approximation of a BLDC motor under field-oriented control), the voltage-to-angular-velocity transfer function is:

G(s) = ω(s) / V(s) = K / ((J·s + b)(L·s + R) + K²)

Where:

• K = torque/back-EMF constant (N·m/A)
• J = rotor inertia (kg·m²)
• b = viscous friction (N·m·s)
• R, L = armature resistance (Ω) and inductance (H)

When L is small (electrical dynamics much faster than mechanical), this reduces to a first-order system:

G(s) ≈ K_m / (τ·s + 1)
K_m = K / (b·R + K²)        (DC gain)
τ   = J·R / (b·R + K²)      (mechanical time constant)

Identify Parameters From a Step Test

Apply a fixed 12V step, log velocity at 1 kHz, and fit the curve. The time constant τ is the time to reach 63.2% of steady state. K_m is the steady-state velocity divided by the input voltage.

State-Space Form

Modern control uses first-order matrix equations:

ẋ = A·x + B·u
y = C·x + D·u

Where x is the state vector (position, velocity), u is the input (voltage or torque), and y is the measured output.

Stage 2: PID Control

PID is the workhorse of robot joints, wheel drives, and gimbals.

The Control Law

u(t) = Kp·e(t) + Ki·∫e(τ)dτ + Kd·de(t)/dt
e(t) = r(t) − y(t)

Discrete PID With Anti-Windup (Python)

class PID:
    def __init__(self, kp, ki, kd, dt, u_min, u_max, tau_d=0.01):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.dt = dt                      # Sample period in seconds (0.001 = 1 kHz)
        self.u_min, self.u_max = u_min, u_max  # Actuator saturation limits
        self.tau_d = tau_d                # Derivative low-pass filter time constant
        self.integral = 0.0
        self.prev_meas = 0.0
        self.d_filt = 0.0

    def update(self, setpoint, measurement):
        error = setpoint - measurement

        # Derivative on measurement avoids "derivative kick" on setpoint steps
        raw_d = -(measurement - self.prev_meas) / self.dt
        # First-order low-pass filter limits noise amplification
        alpha = self.dt / (self.tau_d + self.dt)
        self.d_filt += alpha * (raw_d - self.d_filt)
        self.prev_meas = measurement

        # Candidate output before clamping
        u_unsat = (self.kp * error
                   + self.ki * self.integral
                   + self.kd * self.d_filt)
        u = max(self.u_min, min(self.u_max, u_unsat))

        # Conditional integration: only accumulate if not saturated
        # or if the error would pull the output back inside limits
        if u == u_unsat or (u_unsat * error) < 0:
            self.integral += error * self.dt
        return u

Tuning Methods Compared

Ziegler-Nichols closed-loop rules for a PID: Kp = 0.6·Ku, Ti = Tu/2, Td = Tu/8, where Ki = Kp/Ti and Kd = Kp·Td.

Cascaded Loop Structure

Servo drives nest three loops, each roughly 5–10x slower than the one inside it:

Tune from the inside out. Close the current loop first, then velocity, then position.

Stage 3: Stability and Frequency-Domain Analysis

Tuning by trial and error fails on systems with delay or resonance. Frequency-domain tools tell you how close the loop is to instability.

Key Concepts

• Gain margin (GM): how much loop gain can rise before instability. Target > 6 dB.
• Phase margin (PM): extra phase lag tolerated at the crossover frequency. Target 45°–60°.
• Bandwidth: frequency where closed-loop gain drops 3 dB. It sets how fast the robot responds.
• Nyquist criterion: closed-loop stability from the open-loop L(jω) encirclements of −1.

Python Margin Check

import control as ct
import numpy as np

# First-order motor plant: K_m = 2.0, tau = 0.15 s
G = ct.tf([2.0], [0.15, 1])
# PID controller in parallel form: Kp=1.5, Ki=4.0, Kd=0.02
C = ct.tf([0.02, 1.5, 4.0], [1, 0])
L = C * G                       # Open-loop transfer function

gm, pm, wcg, wcp = ct.margin(L) # Gain margin, phase margin, crossover freqs
print(f"Gain margin: {20*np.log10(gm):.1f} dB at {wcg:.1f} rad/s")
print(f"Phase margin: {pm:.1f} deg at {wcp:.1f} rad/s")

Sample Rate and Delay

Digital control adds a half-sample delay plus computation time. As a rule, sample at 10–30x the closed-loop bandwidth. Slower sampling erodes phase margin and invites oscillation.

Stage 4: State-Space Control and LQR

PID treats each joint as an independent SISO loop. Multi-joint arms, balancing robots, and drones have coupled dynamics that need MIMO methods.

State Feedback

With full state feedback u = −K·x, the closed-loop dynamics become:

ẋ = (A − B·K)·x

Choosing K places the closed-loop poles. Pole placement works, but it gives no principled way to trade effort against performance.

Linear Quadratic Regulator (LQR)

LQR minimizes a quadratic cost:

J = ∫ (xᵀ·Q·x + uᵀ·R·u) dt

• Q penalizes state error (larger Q = tighter tracking).
• R penalizes control effort (larger R = gentler, lower-energy actuation).

The optimal gain is K = R⁻¹·Bᵀ·P, where P solves the continuous algebraic Riccati equation:

Aᵀ·P + P·A − P·B·R⁻¹·Bᵀ·P + Q = 0

LQR for an Inverted Pendulum on a Cart

import numpy as np
from scipy.linalg import solve_continuous_are

# Linearized cart-pole about the upright equilibrium
# State x = [cart_pos, cart_vel, pole_angle, pole_ang_vel]
M, m, l, g = 0.5, 0.2, 0.3, 9.81   # kg, kg, m, m/s^2
A = np.array([[0, 1, 0, 0],
              [0, 0, -m*g/M, 0],
              [0, 0, 0, 1],
              [0, 0, (M+m)*g/(M*l), 0]])
B = np.array([[0], [1/M], [0], [-1/(M*l)]])

Q = np.diag([10, 1, 100, 1])        # Heavily penalize pole angle error
R = np.array([[0.1]])               # Moderate control effort cost

P = solve_continuous_are(A, B, Q, R)    # Solve the Riccati equation
K = np.linalg.inv(R) @ B.T @ P          # Optimal gain: u = -K x
print("LQR gain K =", K)

Observers and the Kalman Filter

Real robots cannot measure every state. An observer estimates hidden states from sensor data. The Kalman filter is the optimal observer for linear systems with Gaussian noise. Pairing LQR with a Kalman filter gives LQG control, and it ties directly to IMU and encoder sensor fusion.

Stage 5: MPC and Advanced Methods

Model Predictive Control (MPC) solves a finite-horizon optimization at every step and applies only the first control input:

minimize   Σ (xₖᵀ·Q·xₖ + uₖᵀ·R·uₖ)      for k = 0 … N−1
subject to xₖ₊₁ = A·xₖ + B·uₖ
           u_min ≤ uₖ ≤ u_max             (actuator limits)
           x_min ≤ xₖ ≤ x_max             (joint and safety limits)

MPC handles constraints explicitly, which LQR and PID cannot. That makes it the standard for legged locomotion, drone trajectory tracking, and autonomous mobile robot (AMR) path following.

Controller Selection Guide

Other topics worth studying once MPC is comfortable: computed torque (inverse dynamics) control for manipulators, impedance control for contact tasks, and adaptive control for payload changes.

Deployment With ROS2 ros2_control

ros2_control separates hardware interfaces from controllers and runs them in a deterministic loop.

Controller Configuration

# controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 1000            # Control loop frequency in Hz

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

joint_trajectory_controller:
  ros__parameters:
    joints: [joint1, joint2]
    command_interfaces: [position]   # What the controller writes to hardware
    state_interfaces: [position, velocity]  # What it reads back
    gains:
      joint1: {p: 80.0, i: 0.5, d: 4.0, i_clamp: 10.0}  # PID per joint
      joint2: {p: 60.0, i: 0.4, d: 3.0, i_clamp: 10.0}

Launch and Verify

# Load and start the controllers
ros2 control load_controller --set-state active joint_state_broadcaster
ros2 control load_controller --set-state active joint_trajectory_controller

# Confirm both are active
ros2 control list_controllers

# Inspect measured loop rate and jitter
ros2 topic hz /joint_states

Run the controller manager on a PREEMPT_RT kernel. Jitter above 100 µs at 1 kHz degrades the effective phase margin.

Real-World Workflow: Tuning a Velocity Loop for a Differential-Drive Robot

1. Characterize the plant. Drive each wheel with a 0 → 6V step. Log encoder velocity at 1 kHz for 2 seconds. Fit K_m and τ.
2. Set the sample rate. Choose 1 kHz, comfortably above 20x the expected 15 Hz closed-loop bandwidth.
3. Start with P only. Raise Kp until the response is fast with mild overshoot.
4. Add Ki. Remove steady-state error. Add the anti-windup clamp from the code above.
5. Add a small Kd only if encoder quantization noise is filtered. Otherwise skip it.
6. Check margins with the ct.margin script. Confirm PM > 45°.
7. Test under load. Add a 1 kg payload and verify rise time, overshoot, and saturation behavior.
8. Close the outer loop. Use the tuned velocity loops under a heading controller or a Pure Pursuit path follower.

Recommended Study Path and Resources

Pair theory with simulation first (Gazebo, MuJoCo, or Python control), then move to hardware. Simulated loops hide sensor noise, quantization, and delay, so always validate on a physical rig before trusting the gains.

FAQ

What math do I need for control theory in robotics?

Linear algebra (matrices, eigenvalues), differential equations, and Laplace transforms cover most of it. Add basic probability for Kalman filtering and convex optimization for MPC.

Should I learn PID or modern control first?

Learn PID first. It is the dominant industrial controller, and it builds the intuition (error, gain, damping, windup) that LQR and MPC extend.

What is the difference between PID and LQR?

PID acts on the output error of a single loop and needs tuning by hand or heuristics. LQR uses the full state and a system model to compute a gain that minimizes a quadratic cost, and it handles coupled multi-input systems directly.

What software should I use to practice?

Python with the control and scipy libraries covers analysis and design. MATLAB/Simulink is common in industry. For deployment, use ROS2 with ros2_control and test in Gazebo or MuJoCo.