Robotics · A Visual Primer

Path Tracking, Arm & Aerial Navigation

Now that the robot knows where it is, how does it actually get somewhere?

By Majid Mazouchi

1 — Three layers of robot motion

A robot that knows where it is and what's around it (the previous primer) still has to actually move. Motion is built in layers, from highest level down to physics:

Mission planning — what to do. Pick up that box, then dock at station B.
Path planning — what shape to follow. Compute a feasible curve from here to there avoiding obstacles.
Path tracking / control — how to actually follow it. Given the planned path and the current state, what command (steering, joint torque, thrust) do I send to the motors right now?

This primer is about layer 3. Same problem appears across very different bodies — wheeled cars, robotic arms, drones, rockets — and each has its own favorite controller. The math turns out to be surprisingly similar; the differences are mostly in the body model and the actuator constraints.

The unifying idea

Every controller in this primer answers the same question: given a desired state x* and the current state x, what control input u minimizes the deviation while respecting the body's dynamics? The differences are in (a) how the deviation is measured (lateral error? joint angle error? attitude quaternion?), (b) what model is used (kinematic? dynamic? linearized?), and (c) whether constraints matter (steering limits, joint limits, thrust limits).

Reference implementation

Every algorithm here is implemented in AtsushiSakai/PythonRobotics. We use its taxonomy: move-to-pose, Stanley, rear-wheel feedback, LQR, MPC, NMPC with C-GMRES, two/N-joint arm control, arm with obstacle avoidance, drone 3D trajectory, and rocket powered landing.

2 — Path tracking: "Follow that line."

You have a smooth path through the world (planned by RRT, A*, or Frenet sampling). The car has to drive along it. The path is geometric — points in space — and the car has dynamics (it can't turn instantly, can't slip sideways, can't accelerate infinitely). Path tracking is the bridge.

The simplest model is the kinematic bicycle: two wheels (front steers, rear drives), wheelbase L, position (x, y), heading θ, speed v, steering angle δ. The motion equations are:

ẋ = v cos θ ẏ = v sin θ θ̇ = (v / L) tan δ

That's the body. The controller's job is to pick δ (and maybe acceleration) at every time step.

Method 1 — Move to a pose polar coordinate controller

The simplest path-following problem isn't path-following at all: just go from where you are to a specific (x, y, θ) goal pose. No path between, no obstacles. Useful for parking, docking, last-meter delivery.

The trick is to express the geometry in polar coordinates relative to the goal:

ρ — distance from robot to goal
α — angle from the robot's heading to the goal point
β — angle between the goal heading and the line from robot to goal

Then drive ρ down with linear speed, and use angular speed to align α and β:

v = K_ρ · ρ ω = K_α · α + K_β · β

This system is asymptotically stable when K_ρ > 0, K_β < 0, K_α − K_ρ > 0. Tune the gains and it just works.

Demo 1 — Move to a pose, click to set the goal

Click anywhere on the canvas to place a goal pose. The arrow shows the goal heading. The car drives there using the polar-coordinate controller. Tune the gains and watch how the path shape changes — high K_ρ makes it dive straight in, balanced gains produce graceful curves.

K_ρ K_α K_β

Robot Goal pose Trail

ρ = — α = — β = — v = — ω = —

Method 2 — Geometric path trackers Pure Pursuit, Stanley, Rear-wheel feedback

For an actual path (not just a goal pose), the geometric controllers are the workhorses. They look at the path, find a relevant point on it, and compute a steering angle that geometrically points the car at that point. They are simple, fast, and surprisingly hard to beat.

Pure Pursuit

Pick a point on the path a fixed look-ahead distance L_d ahead of the rear axle. Steer toward it using:

δ = atan( 2L sin α / L_d )

where α is the angle from the rear axle to the lookahead point and L is the wheelbase. Geometrically, the car drives along a circular arc that passes through the lookahead point. Larger L_d → smoother but laggier tracking.

Stanley

Used by the DARPA Grand Challenge winner. Path tracking simulation with Stanley steering control and PID speed control was used by Stanley, the robot that won the DARPA grand challenge. Stanley looks at the front axle and combines two errors:

δ = θ_e + atan( k · e / v )

where θ_e is the heading error (vehicle vs path direction), e is the cross-track error (signed perpendicular distance from front axle to path), and k is a gain. The first term aligns the vehicle's heading with the path; the second term steers in to kill cross-track error. The 1/v term gives the controller stronger correction at low speed.

Rear-wheel feedback

A more principled controller derived from Lyapunov analysis on the path-frame error dynamics. It explicitly accounts for the path's curvature κ and uses both heading and lateral errors with a stable feedback law. Tracks tightly and smoothly when curvature is well-estimated.

Demo 2 — Three geometric path trackers, side by side

A bicycle-model car tracks a wavy reference path (gray). Switch between Pure Pursuit, Stanley, and Rear-wheel feedback to see how each behaves at high speed. Watch the cross-track error chart: Stanley's 1/v term over-corrects at low speed; Pure Pursuit at large lookahead is smooth but laggy; rear-wheel feedback is the most balanced.

Controller Speed Gain

Reference path Vehicle trajectory Lookahead / target

cross-track e = — heading err = — steering δ = — RMS error = —

Which one to use?

Pure Pursuit when you want simplicity and don't care about overshoot — ag robots, lawn mowers, slow ground robots. Stanley when you need tight tracking at moderate-to-high speeds and the path is reasonably smooth — autonomous cars on roads. Rear-wheel feedback when you have an accurate curvature estimate and want the smoothest control with the best stability margins — competition AVs and racing.

Method 3 — Optimal control: LQR & MPC model-based

Geometric controllers are reactive — they look at one point on the path and respond. Optimal controllers are predictive: they have a model of the vehicle, look ahead a horizon, and pick the control that minimizes a cost over that horizon.

LQR steering control

Linearize the lateral error dynamics around the path. State x = [e, ė, θ_e, θ̇_e]. Discrete-time linear system x_{t+1} = A x_t + B u_t. Pick a quadratic cost:

J = Σ_t ( x_tᵀ Q x_t + u_tᵀ R u_t )

The LQR solution is a constant feedback gain u = −K x, where K comes from solving the discrete-time algebraic Riccati equation (DARE). LQR speed and steering control extends this with velocity error v_err so the same cost function jointly handles longitudinal and lateral tracking. Q penalizes state deviation; R penalizes control effort. Tune the diagonal entries of Q and R to taste.

Model Predictive Control (MPC)

The same idea, but with two key additions: a finite horizon (re-solve every step, only execute the first control), and explicit constraints. At each step, solve:

min Σ_{k=0}^{N-1} ( x_kᵀ Q x_k + u_kᵀ R u_k ) + x_Nᵀ Q_f x_N
s.t. x_{k+1} = A x_k + B u_k, x_0 = x_now, |δ| ≤ δ_max, |a| ≤ a_max

The constraints matter — LQR ignores them and can request 90° steering angles. MPC respects the steering limits and the friction circle. The cost is at every step you solve a small QP (quadratic program).

Nonlinear MPC with C-GMRES

If the dynamics are too nonlinear to linearize cleanly (sharp turns, heavy braking, drift conditions), the linearization in MPC breaks down. NMPC keeps the nonlinear model directly and solves a more expensive optimization. C-GMRES is a continuation/Krylov-iterative solver that keeps NMPC fast enough for real-time use on cars and rockets — it warm-starts from the previous solution and uses iterative linear algebra instead of a full Newton step.

Demo 3 — LQR vs MPC tracking on the same path

Same car, same reference path. Toggle between LQR (constant feedback gain) and MPC (predicted trajectory shown in orange dots). Watch how MPC anticipates upcoming curvature — its trajectory bends before reaching the curve, while LQR only reacts. Tighten the steering limit and you'll see MPC respect it; LQR can't.

Controller MPC horizon |δ| max

Reference path Vehicle trajectory MPC predicted horizon

cross-track e = — steering δ = — RMS error = —

Methods at a glance

Method	Type	Strengths	Weaknesses
Move to a pose	Polar feedback	Trivial to implement; works for any final pose	Single point only; no obstacle handling
Pure Pursuit	Geometric	Smooth; very robust; tunes with one parameter	Cuts inside corners; lags at high speed
Stanley	Geometric	Tight tracking; DARPA-grade; intuitive errors	1/v term is fragile near zero speed
Rear-wheel feedback	Lyapunov	Best stability; smooth on curved paths	Needs accurate curvature; trickier to tune
LQR (steering / speed+steering)	Optimal, infinite horizon	Optimal under linear model; constant gain → fast	Ignores constraints; linearization assumption
MPC	Optimal, receding horizon	Handles constraints (steering, friction, speed); anticipates	Solves a QP each step; tuning is high-dim
NMPC + C-GMRES	Nonlinear, real-time	Full nonlinear dynamics; constraints; real-time	Implementation complexity; warm-start matters

Use cases

Autonomous cars on highways. MPC is industry standard. Tesla AP, Mobileye, Waymo all use MPC variants for lateral and longitudinal control.
Self-parking and valet. Move-to-pose for the final docking; geometric trackers for the path.
Warehouse AGVs / AMRs. Pure Pursuit is overwhelmingly common — simple, robust, predictable.
Racing and high-performance driving. NMPC dominates because constraint handling on the friction circle matters.
Agricultural robots. Stanley or Pure Pursuit on planned coverage paths.

Practical notes

Watch out for

Speed-dependent gains. Most controllers need their gains scheduled with speed. A Stanley gain that's perfect at 5 m/s will oscillate at 30 m/s and lag at 0.5 m/s. Schedule with v.

Reference smoothness. Geometric controllers misbehave on jagged paths. Always smooth your planner output — splines, polynomial fits, or short-horizon optimization.

MPC tuning is hard. Q and R have huge dynamic ranges. A common trick is to normalize each state by its acceptable error and each control by its limit, then start with diagonal Q = R = I and adjust.

Pure-pursuit cuts corners. Even with proper tuning, it geometrically cuts inside curves. For lane-following, account for the offset by inflating the path away from the inside of curves, or switch to Stanley.

Real-time NMPC needs warm-starting. Solving the full nonlinear program from scratch every 20 ms is too slow. C-GMRES, sequential quadratic programming with warm starts, or differential dynamic programming are the practical solvers.

3 — Arm navigation: "Reach there with this many joints."

A robot arm has multiple revolute joints chained together. Given joint angles q = (q₁, q₂, ..., qₙ), the end-effector pose is computed by chaining the link transforms — that's forward kinematics. The hard problem is the inverse: given a desired end-effector position, what joint angles get me there? That's inverse kinematics (IK).

Method 1 — N-joint arm to a point inverse kinematics

For a 2-joint arm in the plane, IK has a closed-form: it's just the law of cosines. But beyond two joints — and especially for redundant arms (more joints than dimensions) — there's no closed form, and you need a numerical method.

Two of the most-used numerical methods:

Cyclic Coordinate Descent (CCD) — one joint at a time, from the tip back to the base, rotate that joint so the line from joint to end-effector points at the target. Repeat until close. Simple, fast, no Jacobian needed.
Jacobian pseudoinverse — write down the Jacobian J = ∂(end-effector pos)/∂q. Apply the rule q ← q + J⁺ · (target − end-effector pos). This is gradient descent in joint space. Works in 3D and handles redundancy elegantly.

Demo 4 — N-joint arm reaching to a point

A planar arm with N revolute joints. Click anywhere in the workspace to set the target. CCD inverse kinematics solves for the joint angles. Add joints with the slider — watch the redundancy: with more joints than needed, the arm can reach the same point in many configurations.

Joints (N) IK speed

Arm links Joints End effector Target

iterations 0 distance to target — reached? no

Method 2 — Arm navigation with obstacle avoidance redundancy resolution

If the workspace has obstacles, IK alone isn't enough — the arm may pass through walls on its way to the target. The classical fix is to add a secondary task: while the primary task drives the end-effector to the target, the secondary task pushes the whole arm away from obstacles. Redundant arms (more DOF than the task needs) have null-space room to do both.

The simple practical recipe is artificial potential fields: the goal pulls the end-effector with an attractive force, and each obstacle repels every link with a force inversely proportional to distance. Sum the forces, project them through the Jacobian, and step the joints.

Demo 5 — Arm reaching with obstacles in the way

Same N-joint arm. Now click sets the target as before, but the workspace has obstacles (gray disks). The arm uses an attractive force from the goal plus repulsive forces from each obstacle on every link, projected through the Jacobian. The arm bends around obstacles to reach the target.

Repulsion

Arm Target Obstacles

distance to target — min obstacle distance —

When potential fields fail

Potential fields are easy and intuitive, but they have a famous failure: local minima. If the attractive and repulsive forces balance somewhere that isn't the goal, the arm just stops there. The standard fixes are: (a) layer a global planner like RRT or RRT* on top to provide intermediate waypoints, (b) use harmonic potential fields (Laplace's equation), or (c) switch to gradient-flow trajectory optimization (TrajOpt, CHOMP, STOMP).

Use cases

Industrial pick-and-place. 6-DOF arms picking parts off a conveyor — IK plus collision-aware planning.
Surgical robots. Da Vinci-style systems combine high-DOF redundancy with virtual fixtures and remote-center-of-motion constraints.
Bin-picking. Vision finds the object pose; IK + collision checking computes a path; controller executes.
Humanoid manipulation. Whole-body control with redundancy: reach with the hand while keeping balance.

4 — Aerial navigation: "Now do it in 3D, with thrust."

Drones and rockets share a common difficulty: they must hold themselves up against gravity while navigating. Their actuators are thrusters and gimbals, not wheels or joints. They are underactuated — fewer independent control inputs than degrees of freedom — which means you cannot steer attitude and position independently.

Method 1 — Drone 3D trajectory following geometric controller

A quadrotor has four rotor speeds and six DOF (three position, three attitude). To track a 3D trajectory, the standard architecture is a cascade:

Position controller — desired position → desired acceleration → desired thrust direction.
Attitude controller — desired orientation → desired body rates → desired torques.
Motor mixer — desired thrust + torques → individual rotor speeds.

The famous Lee, Leok & McClamroch geometric controller does this in SO(3) directly, avoiding gimbal lock and giving exponential stability over almost the whole rotation manifold. Almost every research drone runs some version of it.

Demo 6 — Quadrotor following a 3D trajectory

A quadrotor (red marker) follows a 3D trajectory (gray helix or figure-8). The wireframe view rotates the scene so you can see the motion in 3D. Switch trajectories with the dropdown. The blue arrow is the thrust vector — note how it tilts to produce horizontal acceleration.

Trajectory Speed

Reference trajectory Drone Thrust vector

position error — m tilt angle — thrust —

Method 2 — Rocket powered landing trajectory optimization

Landing a rocket on a target — Falcon 9, lunar landers, Mars EDL — is a constrained trajectory optimization problem. You have:

Dynamics — 6-DoF rigid body with thrust along the body axis, mass decreasing as fuel burns.
Initial state — high altitude, descending.
Terminal state — touchdown at the target with zero velocity, body upright.
Constraints — minimum and maximum thrust, gimbal angle limits, glide slope (don't fly under the ground), pointing constraints (radar uplink), fuel limit.
Cost — minimum fuel, or minimum landing error.

The breakthrough was the convex relaxation by Açıkmeşe and others: with a clever change of variables and a lossless relaxation of the lower-thrust bound, the entire problem becomes a second-order cone program (SOCP) that can be solved to global optimum in milliseconds. This is what flies on Falcon 9. The "G-FOLD" (Guidance for Fuel-Optimal Large Diverts) algorithm is a JPL-developed extension used for Mars landings.

Demo 7 — Powered descent trajectory to a target

A rocket descends from altitude. Click anywhere on the ground to set a landing target — the rocket diverts to it within fuel and thrust constraints. The orange envelope shows the glide slope. The thrust vector (blue) is the optimization variable; it points to balance gravity and steer the rocket. Watch the trajectory plan get re-solved on each click.

Initial altitude Initial vx

Rocket Landing target Thrust vector Planned trajectory Glide slope

altitude — m velocity — m/s thrust — % fuel — %

The convex-relaxation trick, in one paragraph

The thrust constraint is non-convex: T_min ≤ |T| ≤ T_max with T_min > 0 means the thrust vector lives in a shell, not a ball. But if you introduce a slack variable σ with |T| ≤ σ and T_min ≤ σ ≤ T_max, the constraint becomes convex (a second-order cone plus a box). Açıkmeşe proved that the optimum of the relaxed problem always satisfies |T| = σ at every time — the relaxation is lossless. Now you can solve in milliseconds with off-the-shelf SOCP solvers.

Use cases

SpaceX Falcon 9 landings. Real-time convex trajectory optimization on the booster's flight computer.
Quadrotor racing and acrobatics. Geometric controllers with feed-forward terms; some teams now use learned policies on top.
Mars EDL. JPL's G-FOLD and successors. Curiosity, Perseverance, and lunar landers use related convex-optimization-based descent guidance.
Drone delivery and inspection. Trajectory generation via minimum-snap polynomials, tracked by geometric controllers.
VTOL aircraft (eVTOL air taxis). Hybrid aerial vehicles use related but more complex multi-mode controllers.

5 — Side by side

	Path tracking	Arm navigation	Aerial navigation
Body	Wheeled (bicycle, Ackermann, differential)	Serial chain of revolute / prismatic joints	Quadrotor / fixed-wing / rocket
State	(x, y, θ, v)	q = (q₁, ..., qₙ) joint angles	(p, v, R, ω) — 12 to 13 dimensions
Control	Steering δ + acceleration a	Joint torques τ or velocities q̇	Thrust T + body torques M
Underactuated?	Yes (can't move sideways)	No (if non-redundant); free DOF if redundant	Yes (4 controls, 6 DOF)
Dominant method	MPC + geometric trackers	Jacobian-based IK + trajectory opt.	Geometric SO(3) control + convex traj. opt.
Hard part	Speed-dependent dynamics; tire slip	Singularities; redundancy resolution	Underactuation; thrust-mass coupling
Library	PythonRobotics path_tracking	PythonRobotics arm_navigation	PythonRobotics aerial_navigation

6 — Practical engineering notes

Pick the right model fidelity

For most ground vehicles, the kinematic bicycle is enough — it ignores tire slip and lateral dynamics, but it's accurate below ~7 m/s and easy to invert. Above that, you need the dynamic bicycle (slip angles, cornering stiffness) or, on real cars, a 4-wheel model with weight transfer. For arms, kinematic IK is enough for slow motion; full dynamics (M(q)q̈ + C(q,q̇)q̇ + g(q) = τ) matters for high-speed motion or compliance control. For drones, the linearized hover model works near hover; geometric SO(3) is needed for aggressive flight.

Always close two loops

Almost every working robot has a two-tier control hierarchy: an outer loop at 10–100 Hz computing high-level commands (steering angle, end-effector velocity, attitude setpoint), and an inner loop at 100–1000 Hz tracking those commands tightly (servo control, motor PID, rate gyro). The outer loop can be slow and smart; the inner loop must be fast and dumb.

Constraints aren't optional

Steering limits, joint limits, thrust limits, voltage saturation — these aren't suggestions. A controller that ignores constraints will eventually demand the impossible, integrator-wind-up, and oscillate. MPC handles constraints natively. PID-style controllers need explicit anti-windup and saturation logic. LQR doesn't handle them at all — pair it with a saturator.

Test in sim first

The PythonRobotics simulations are not toys; they're the bare minimum testing harness you need before touching real hardware. Build a sim of your robot's dynamics (or use Gazebo / Isaac Sim / MuJoCo for serious work), tune your controller against the sim, then port. Code that's never been simulated will surprise you on the real robot.

Trajectory generation matters as much as tracking

A perfect tracker on a bad path produces a bad result. Smooth your reference: cubic splines for waypoints, minimum-jerk for arms, minimum-snap polynomials for drones, convex optimization for rockets. PythonRobotics has reference implementations of cubic, B-spline, and Bézier paths that pair naturally with these trackers.

7 — References & further reading

Code (start here)

Sakai, A. et al. PythonRobotics — runnable Python implementations of every algorithm in this primer: PathTracking/, ArmNavigation/, and AerialNavigation/ directories. The N-joint arm to a point control is an interactive simulation where the goal position of the end effector can be set with a left-click on the plotting area, with N = 10 by default.
Sakai, A., Ingram, D., et al. "PythonRobotics: a Python code collection of robotics algorithms." arXiv:1808.10703 (2018).

Path tracking

Coulter, R. C. "Implementation of the Pure Pursuit Path Tracking Algorithm." Robotics Institute, CMU, 1992. — The original Pure Pursuit tech report.
Hoffmann, G. M., Tomlin, C. J., Montemerlo, M., & Thrun, S. "Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing." ACC, 2007. — Stanley's controller paper.
Snider, J. M. "Automatic Steering Methods for Autonomous Automobile Path Tracking." Tech. Rep. CMU-RI-TR-09-08, 2009. — Comprehensive survey including Pure Pursuit, Stanley, and feedback methods.
Falcone, P., Borrelli, F., et al. "Predictive Active Steering Control for Autonomous Vehicle Systems." IEEE T-CST, 2007. — A foundational MPC-for-cars reference.
Ohtsuka, T. "A continuation/GMRES method for fast computation of nonlinear receding horizon control." Automatica, 2004. — The C-GMRES paper.
Kong, J., Pfeiffer, M., Schildbach, G., & Borrelli, F. "Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design." IV, 2015.

Arm navigation

Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. — The standard arm-control textbook.
Lynch, K. M. & Park, F. C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. — Free PDF; clear treatment of forward / inverse kinematics and Jacobian methods.
Wang, L. & Chen, C. "A Combined Optimization Method for Solving the Inverse Kinematics Problem of Mechanical Manipulators." IEEE T-RA, 1991. — Cyclic Coordinate Descent foundations.
Khatib, O. "Real-time Obstacle Avoidance for Manipulators and Mobile Robots." IJRR, 1986. — The original artificial potential fields paper.
Ratliff, N., Zucker, M., Bagnell, J. A., & Srinivasa, S. "CHOMP: Gradient Optimization Techniques for Efficient Motion Planning." ICRA, 2009. — Modern trajectory optimization for arms.

Aerial navigation

Mellinger, D. & Kumar, V. "Minimum Snap Trajectory Generation and Control for Quadrotors." ICRA, 2011. — The minimum-snap reference for drones.
Lee, T., Leok, M., & McClamroch, N. H. "Geometric Tracking Control of a Quadrotor UAV on SE(3)." CDC, 2010. — The geometric controller used in almost every research drone.
Açıkmeşe, B. & Ploen, S. R. "Convex Programming Approach to Powered Descent Guidance for Mars Landing." JGCD, 2007. — The convex-relaxation breakthrough for rocket landing.
Blackmore, L., Açıkmeşe, B., & Scharf, D. P. "Minimum-Landing-Error Powered-Descent Guidance for Mars Landing Using Convex Optimization." JGCD, 2010. — G-FOLD.
Mao, Y., Szmuk, M., & Açıkmeşe, B. "Successive Convexification of Non-Convex Optimal Control Problems and its Convergence Properties." CDC, 2016. — Modern successive-convexification approach.

Books worth owning

Corke, P. Robotics, Vision and Control. Springer, 3rd ed., 2023. — The most readable broad survey, with MATLAB and Python toolboxes.
Rawlings, J. B., Mayne, D. Q., & Diehl, M. M. Model Predictive Control: Theory, Computation, and Design. 2nd ed., Nob Hill, 2017. — The canonical MPC textbook. Free PDF.
Bryson, A. E. & Ho, Y. C. Applied Optimal Control. Hemisphere, 1975. — Classical optimal control; LQR derivation par excellence.

Online courses

Davide Scaramuzza — UZH Robotics & Perception lectures (drones).
Russ Tedrake — MIT 6.832 "Underactuated Robotics" — the canonical lectures and free book on optimal control for robots.
Steve Brunton — YouTube control bootcamp series. Friendly intro to LQR and MPC.
Coursera Self-Driving Cars Specialization (University of Toronto) — Includes hands-on Python labs for Stanley, Pure Pursuit, and MPC.

Open-source codebases

acados — Real-time embedded MPC; used in production AVs and drones.
CasADi — Symbolic framework for nonlinear optimization and optimal control.
OSQP / ECOS / SCS — Open-source QP and SOCP solvers, widely used in MPC.
PX4 / ArduPilot — Production drone autopilots; geometric attitude control is in the source.
MoveIt! — ROS arm motion planning framework.
Drake (Toyota Research) — Multibody dynamics + trajectory optimization; the Tedrake group's research codebase.