Optimal control: the linear quadratic regulator

Pole placement answers the question 'where should the poles go?' with the engineer's judgement, and for systems with many states and inputs that judgement becomes hard to exercise. Placing the four poles of a flexible joint is manageable; placing the twelve poles of a six-axis arm's linearized model, or of a balancing robot with several inputs, is not. The linear quadratic regulator, LQR, replaces pole locations with a more natural specification: a cost that penalizes state error and control effort, which the controller then minimizes. The result is a state-feedback gain with excellent robustness, computed reliably by software for systems of any size. LQR is used in balancing and legged robots, drones, satellite attitude control and as the inner layer of model predictive control. This lesson explains the cost function, the Riccati equation that solves it, how to choose the weights, and how to compute the gains numerically.

The quadratic cost

For the linear system \( \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \), LQR finds the input that minimizes

The weighting matrix Q, symmetric and positive semidefinite, sets how much each state error costs; R, symmetric and positive definite, sets how much control effort costs. A large Q relative to R asks for aggressive control that drives errors to zero quickly at the price of large torques; a large R asks for gentle control that saves energy and avoids saturation at the price of slower response. The quadratic form is chosen partly for its mathematical convenience, but it also matches engineering intuition: small errors matter little, large errors matter a lot, and torque costs energy roughly in proportion to its square, since motor copper loss is \( i^2R \).

The solution: the Riccati equation

Remarkably, the optimal input for this infinite-horizon problem is a constant linear state feedback,

where P is the symmetric positive-definite solution of the continuous algebraic Riccati equation (CARE):

The matrix P has a physical meaning: the minimum achievable cost starting from state \( \mathbf{x}_0 \) is \( \mathbf{x}_0^{\mathsf{T}}\mathbf{P}\mathbf{x}_0 \). It acts as a cost-to-go, and it also serves as a Lyapunov function proving that the closed loop is stable. A solution exists and gives a stable closed loop when (A, B) is controllable (or at least stabilizable) and every unstable mode is penalized by Q (detectability). Only for small systems can the Riccati equation be solved by hand; normally it is solved numerically.

LQR for the double integrator

A lightly damped joint is close to a double integrator, \( \ddot{q} = u \) with u the torque divided by inertia. With \( \mathbf{Q} = \text{diag}(q_1, q_2) \) and scalar R = r, the Riccati equation can be solved in closed form, giving

so the closed-loop characteristic polynomial is \( s^2 + k_2s + k_1 \), with \( \omega_n = \sqrt{k_1} = (q_1/r)^{1/4} \) and, when \( q_2 = 0 \), \( \zeta = \sqrt{2}/2 \approx 0.707 \). Without any explicit request for damping, the optimal controller chooses a well-damped response with about 4 % overshoot. The fourth root means the weights must change a lot to change speed: multiplying \( q_1/r \) by 16 only doubles the natural frequency.

Choosing Q and R

The weights are the design knobs, and a systematic starting point is Bryson's rule: make each term in the cost roughly 1 when its variable reaches its largest acceptable value,

For a joint where an error of 0.01 rad and a torque of 50 N·m are each just tolerable, Bryson's rule gives \( Q_{11} = 10^4 \) and \( R = 4 \times 10^{-4} \). From there, tuning is iterative: simulate, look at the response and the peak torque, and scale the weights. Increase a state's weight if it settles too slowly or overshoots too much; increase R if the actuator saturates or the control is too noisy. Off-diagonal terms in Q can penalize combinations of states, such as the twist of a flexible joint, \( (\theta_1 - \theta_2)^2 \).

Robustness of LQR

LQR designs come with guaranteed robustness for single-input systems with full state feedback: a gain margin from 1/2 to infinity (the gain can be halved or increased without limit) and a phase margin of at least 60°. These guarantees follow from a property of the optimal loop called the return difference inequality, and they are one reason LQR is trusted in aerospace and robotics. The guarantees apply at the plant input and assume the full state is measured perfectly. When an observer supplies the state, as in lesson 6, the combined LQG design (LQR with a Kalman filter) can have arbitrarily poor margins, a famous result from 1978, so margins must be checked again; techniques such as loop transfer recovery address this.

Discrete-time LQR and the iterative Riccati solution

Robot controllers run in discrete time. For the sampled system \( \mathbf{x}_{k+1} = \mathbf{A}_d\mathbf{x}_k + \mathbf{B}_d\mathbf{u}_k \), obtained by exact discretization (ROB-230), the cost becomes a sum and the solution is \( \mathbf{u}_k = -\mathbf{K}\mathbf{x}_k \) with

where P solves the discrete algebraic Riccati equation. A simple, robust way to find P is to iterate the Riccati recursion backward from P = Q until it stops changing:

Each step of this recursion is one step of dynamic programming, solving the optimal control problem one more time step from the end. Run for a finite number of steps, it gives the finite-horizon LQR used inside model predictive control; run to convergence, it gives the constant infinite-horizon gain. In discrete time the cost weights are per sample, so Q and R from a continuous design should be scaled by the sample time h, roughly \( \mathbf{Q}_d \approx h\mathbf{Q} \) and \( \mathbf{R}_d \approx h\mathbf{R} \), to get similar behaviour.

import numpy as np
from math import sqrt
h = 0.001                                         # 1 kHz controller
Ad = np.array([[1, h], [0, 1]]); Bd = np.array([[h*h/2], [h]])   # exact ZOH double integrator
Q = np.diag([160000.0, 0.0])*h; R = np.array([[1.0]])*h
P = Q.copy()
for it in range(200000):
    G = np.linalg.solve(R + Bd.T @ P @ Bd, Bd.T @ P @ Ad)
    Pn = Q + Ad.T @ P @ Ad - Ad.T @ P @ Bd @ G
    if np.max(abs(Pn - P)) < 1e-9*np.max(abs(P)): break
    P = Pn
K = np.linalg.solve(R + Bd.T @ P @ Bd, Bd.T @ P @ Ad)
print(f"converged after {it} iterations; discrete K = {np.round(K, 2)}")
print(f"continuous formula: k1 = {sqrt(160000):.1f}, k2 = {sqrt(2*400):.2f}")
z = np.linalg.eigvals(Ad - Bd @ K); s = np.log(z)/h
wn = abs(s[0]); print(f"equivalent poles {np.round(s, 2)}, wn = {wn:.2f}, zeta = {-s[0].real/wn:.3f}")

converged after 544 iterations; discrete K = [[394.38  28.08]]
continuous formula: k1 = 400.0, k2 = 28.28
equivalent poles [-14.14+14.14j -14.14-14.14j], wn = 20.00, zeta = 0.707

The iteration converges in a few hundred steps to gains within about 1.5 % of the continuous closed-form result (394 against 400, and 28.1 against 28.3; the small difference comes from the sample-time scaling of the weights), and the equivalent continuous poles show the expected 20 rad/s natural frequency and damping ratio of 0.707. At 1 kHz the sampling barely matters for a 20 rad/s loop; for faster loops or slower controllers, the discrete design accounts for the sampling exactly where a continuous design would not.

LQR in robotics practice

Three practical patterns recur. First, LQR on a linearized model: balancing robots, quadrotors and legged robots in stance are linearized about an operating point (upright, hovering, standing) and stabilized by LQR, sometimes with gain scheduling across several operating points. Second, LQR around a trajectory: time-varying LQR, from the finite-horizon Riccati recursion along a planned motion, stabilizes deviations from a nominal trajectory computed by an optimizer. Third, LQR as the core of model predictive control (MPC), which re-solves a finite-horizon problem with explicit torque and state limits at every control step; LQR cannot handle such inequality constraints, MPC can, at higher computational cost. In all three, the cost weights are the main design interface, and the Bryson-rule starting point followed by simulation-based tuning is standard practice.