Skip to the lesson
FixTheRobot Explore Robotics / Navigation / M5 Trajectories and Path Following / M5-T6

Navigation · Theory · M5

M5-T6 — LQR and MPC for Path Tracking

Instead of a rule of thumb, use a model of the robot and choose the steering that minimises a cost.

  • Lesson 6 of 7
  • Module 5: Trajectories and Path Following
  • Theory
  • About 8 minutes to read

Pure pursuit and Stanley are rules. LQR and MPC are recipes that start from a model and a cost: how much do we mind being off the path, how much do we mind steering hard? They give gains that come out of the numbers instead of from tuning by eye, and MPC can respect the limits that LQR ignores.

Learning objectives

After this lesson, you should be able to:

  • Write a linearised error model for path tracking and explain its state, input and assumptions.
  • Describe how LQR gives a feedback gain from the weights Q and R, and compute its steering for an error.
  • Describe MPC: prediction, cost, constraints, horizon, receding horizon.
  • Say when each is worth the extra cost over pure pursuit or Stanley.

The intuition

A rule says: when the error is this big, steer this much. A cost says: here is what a bad outcome is, find the best action. Imagine telling Scout "being 10 cm off the path costs one point, turning the wheel 0.3 rad costs one point". LQR finds, once and for all, the steering that gives the best total over an unlimited future when everything is linear. MPC solves a similar problem over the next one or two seconds, every cycle, using the real limits of the robot.

The price is a model. If the model is wrong (the delay is longer than assumed, the wheels slip), the best action for the model is not best for the robot, which is why these controllers are tested for robustness as much as pure pursuit is.

Model-based path trackingA loop: the reference path gives errors; an LQR gain or an MPC optimiser over a horizon turns the errors into a steering command; the robot moves; the pose is measured and compared again. A small sketch shows the MPC horizon of predicted errors.Referencepath + speed Errorse, ψ LQR: u = −K xone gain, no limits MPC: optimise N stepswith limits Robotsteering δ measured pose MPC predicts the errors over the horizon and replans every step
The same loop as before, with the rule replaced by a gain from a cost (LQR) or by an optimisation over a horizon with limits (MPC).

The mathematics

A linearised error model

Take a car-like robot driving at speed v along a straight reference, with the lateral offset e of its rear axle (positive to the left, the opposite sign to the Stanley error of lesson M5-T5), the heading error ψ and the steering angle δ. For small angles:

Error dynamics
e˙=v⋅ψ\dot{e}=v\cdot \psi
ψ˙=(vL)⋅δ\dot{\psi}=(\frac{v}{L})\cdot \delta
L=wheelbaseL=\text{wheelbase}

Over a step dt this becomes a discrete model, xk+1 = A·xk + B·uk with x = (e, ψ) and u = δ:

Discrete error model
A=[1v⋅dt01]A=\begin{bmatrix}1 & v\cdot \mathrm{dt} \\ 0 & 1\end{bmatrix}
B=[0v⋅dtL]B=\begin{bmatrix}0 \\ \frac{v\cdot \mathrm{dt}}{L}\end{bmatrix}

On a curved reference the model is the same plus a feed-forward steering atan(L·κref); the feedback only corrects the error. Because the model depends on speed, the gain is recomputed for each speed (gain scheduling).

LQR

Choose weights Q (how much each error matters) and R (how much steering effort matters) and minimise the infinite-horizon cost:

Discrete LQR
J=∑(xk⊤ Q xk+uk⊤ R uk)J=\sum (x_{k}^{\top}\,Q\,x_{k}+u_{k}^{\top}\,R\,u_{k})
u=−K xu=-K\,x
K=(R+B⊤ P B)−1 B⊤ P AK=(R+B^{\top}\,P\,B)^{-1}\,B^{\top}\,P\,A
P=Q+A⊤ P(A−B K)P=Q+A^{\top}\,P(A-B\,K)
discrete algebraic Riccati equation, solved by iterating

The result is a plain linear feedback, like a PD controller with gains that follow from the model and weights. It is optimal for the model, and it does not know the steering limit: a big error gives a steering command that the robot cannot execute, and the clipped command is no longer optimal.

MPC

  1. Predict. From the current state, use the model to predict the errors N steps ahead for a candidate sequence of steering commands.
  2. Score. Add up the cost of the predicted errors and commands over the horizon.
  3. Constrain. Allow only sequences with |δ| ≤ δmax and a limited steering rate; with a linear model and quadratic cost this is a quadratic programme.
  4. Apply the first command, throw away the rest, repeat at the next cycle (receding horizon).
LQRMPC
ComputationGain computed offlineAn optimisation every cycle (milliseconds)
Limits (steering, rate)Not handledBuilt in
Looks ahead at the pathOnly through feed-forwardYes, over the horizon
Needs a modelYesYes, plus a solver

A worked example

An LQR gain. Take v = 1.0 m/s, wheelbase L = 0.8 m and a step of 0.1 s, so A = [[1, 0.1], [0, 1]] and B = [0, 0.125]ᵀ. With Q = diag(1, 0.1) and R = 1, iterating the Riccati equation to convergence gives

LQR gain for this model
K=[0.922 1.296]K=[0.922\,1.296]
closed−loop poles 0.919±0.070 i\text{closed}-\text{loop poles}\,0.919\pm 0.070\,i
(∣pole∣=0.922 per step)(\lvert \text{pole}\rvert =0.922\,\text{per step})

An offset of 0.30 m with no heading error gives δ = −(0.922 × 0.30) = −0.277 rad = −15.8°, a right turn, as it should be: with e positive to the left, the robot is left of the path. Driven in the model, the error is 0.20 m after 1 s, 0.07 m after 2 s and 0.005 m after 3 s.

When the limit bites. An offset of 1.0 m gives δ = −0.92 rad = 53°. A steering limit of 35° (0.61 rad) clips it. The clipped command is not what the optimisation assumed, and the response is slower and can overshoot. MPC includes the limit in the problem and finds the best sequence that respects it.

Why the horizon must be long enough. Evaluate the cost over N = 3 steps (0.3 s) from the 0.30 m offset, using the same model, Q and R, and adding the final state cost:

Candidate commands (rad)Cost over 3 steps
0, 0, 0 (do nothing)0.360
−0.2, −0.1, 00.405
−0.3, 0, +0.30.534
−0.5, −0.2, 00.639

Over 0.3 s Scout's offset changes by only millimetres, so steering costs more than it gains and doing nothing wins. The controller would never correct an error. A horizon has to be long enough for the benefit of a correction to appear: one to two seconds for a mobile robot, here 10 to 20 steps of 0.1 s.

Engineering implications and limitations

When to use which

For a differential-drive robot on a smooth path at walking speed, regulated pure pursuit is usually enough and needs almost nothing to tune. LQR earns its place when the model is good (a car-like robot at speed) and you want gains that follow from the physics. MPC earns it when limits are active (steering rate, actuator saturation), when the path has tight curvature that requires looking ahead, or when several objectives must be traded together.

Delay in the model

Include the latency in the model (add states for the commands already in the pipeline), or predict the pose forward by the latency before computing the errors. A controller designed with a model that has no delay is the one that weaves in lesson M5-T7.

Real-time and fallback

An MPC that sometimes does not finish in time needs a fallback command. Keep the last good solution, shifted by one step, and a simple controller behind it; monitor the solve time.

Tune weights, not gains

Increase the Q entry of the lateral error for tighter tracking, increase R for gentler steering, and check the result on the step response of lesson M5-T7 before driving.

Common misconceptions

“LQR is optimal, so it cannot fail.”

It is optimal for the linear model without limits. A wrong model, a delay, or a steering limit all invalidate it.

“MPC plans a new path.”

It chooses the control sequence for following the given reference. It does not change the route.

“A longer horizon is always better.”

It costs computation and relies on a model that is accurate that far ahead. A horizon that is too short, however, never corrects an error.

“The gains are fixed.”

In the error model they depend on speed, so they are recomputed or scheduled with speed.

Engineering takeaways

  1. A linearised error model gives x = (e, ψ), u = δ with ė = vψ, ψ̇ = (v/L)δ.
  2. LQR turns the weights Q and R into a gain K through the Riccati equation; the control is u = −Kx.
  3. LQR does not know the steering limit; MPC includes limits, a horizon and a cost, and re-optimises every cycle.
  4. The horizon must be long enough for a correction to pay off (seconds, not tenths of a second).
  5. The model must include the latency and the speed dependence, or the optimum is for the wrong robot.

References and further reading

Standard texts on LQR and MPC, and papers that apply them to vehicle path tracking.

  • B. D. O. Anderson and J. B. Moore, “Optimal Control: Linear Quadratic Methods”, Prentice Hall, 1990.
  • J. B. Rawlings, D. Q. Mayne and M. M. Diehl, “Model Predictive Control: Theory, Computation, and Design”, 2nd edition, Nob Hill Publishing, 2017.
  • P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng and D. Hrovat, “Predictive Active Steering Control for Autonomous Vehicle Systems”, IEEE Transactions on Control Systems Technology, 15(3), 2007.
  • J. Kong, M. Pfeiffer, G. Schildbach and F. Borrelli, “Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design”, IEEE Intelligent Vehicles Symposium, 2015.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

With K = [0.922, 1.296] and the state (e, ψ) = (0.10 m, 0.05 rad), what steering does LQR command?

Explanation

u = −(0.922 × 0.10 + 1.296 × 0.05) = −(0.0922 + 0.0648) = −0.157 rad = −9.0°.

Question 2

Name two things MPC can handle that the LQR gain cannot.

Explanation

Limits on the steering angle and its rate, and looking ahead along the reference over a horizon. It can also trade several costs, such as speed and comfort, in one problem.

Question 3

A horizon of 0.3 s is used and the controller never corrects a small offset. Why?

Explanation

Over 0.3 s the benefit of a correction (a small change of offset) is smaller than the cost of steering, so doing nothing minimises the cost. Lengthen the horizon to one or two seconds.

What’s next

Next: when tracking fails, how to tell whether the path or the controller is to blame.

Next lesson

M5-T7 — When Tracking Goes Wrong

Diagnose oscillation, corner cutting, saturation and latency, and tell planner problems from controller problems.

Nothing below is required to finish this lesson.