A quadrotor racing through gates must hit each waypoint — but also fly smoothly enough that its motors can actually track the path. A straight-line point-to-point plan requires infinite acceleration at every corner: physically impossible. This lesson shows why smooth flight means minimizing high-order derivatives, how polynomial trajectories encode smoothness as a Quadratic Program, and how differential flatness lets you plan in flat output space while guaranteeing the full dynamics are satisfiable. Hand-derived numbers, five interactive canvases, worked QP cost matrices. MIT 16.485 by Luca Carlone, Lectures 8–10.
Picture a drone racing course: five orange gates suspended in mid-air. Your path planner (an RRT* algorithm) hands you five waypoints — the center of each gate. Now you need to generate a trajectory: position as a continuous function of time, from the start to gate 1, gate 2, and so on to the finish.
The naive solution? Fly a straight line to each waypoint in turn. Simple. But watch what happens at each corner: to travel in a straight line, stop, and turn to the next straight line, the drone must decelerate to zero velocity at each waypoint and then accelerate again. That costs time and wastes energy. Worse, what if you want to fly through each gate at full speed? You need the velocity to be continuous — no abrupt stops.
We need a trajectory — not just a path (a sequence of positions) but a function that tells us position at every moment in time, including the velocities, accelerations, and higher derivatives that the drone's actuators must produce. The trajectory must:
The canvas below shows a concrete failure case: the straight-line trajectory and its acceleration profile. See the spikes? Those are the infinite-acceleration corners.
The top view shows a 3-waypoint path. The bottom shows the x-acceleration over time. Toggle between piecewise-linear (sharp corners) and the smooth polynomial version. Watch the accel spikes disappear.
Before we can build a smooth trajectory, we need to make "smooth" precise. Intuitively, a smooth trajectory changes direction gently. Mathematically, smooth means the derivatives of position are small.
Position x(t) is the trajectory itself. Its derivatives have names:
| Derivative | Name | Physical meaning |
|---|---|---|
| dx/dt | Velocity | How fast you move |
| d²x/dt² | Acceleration | Force required (F=ma) |
| d³x/dt³ | Jerk | Rate of force change |
| d4x/dt4 | Snap (crackle) | Rate of jerk change |
A large acceleration means large forces. A large jerk means forces change rapidly, which stresses motors and passengers. A large snap means the higher-order dynamics are excited. We want to fly in a regime where all these derivatives are small.
To get a single number measuring smoothness, we integrate the squared r-th derivative over the flight time T:
Why squared? Because (1) it's always non-negative, (2) squaring penalizes large values more than small ones (a single large spike is penalized more than many small variations adding to the same sum), and (3) it makes the optimization problem quadratic — much easier to solve than absolute value or fourth-power penalties.
Depending on r, we get different optimal trajectories:
Same 3 waypoints, same time budget. Change the derivative order r to minimize. Higher r produces smoother transitions at the cost of larger velocity peaks.
We need a parametric family of functions that can (a) represent any smooth path, (b) be easily differentiated to compute the cost, and (c) have finitely many free parameters to optimize. The answer is polynomials.
Represent the x-coordinate of the trajectory as a degree-N polynomial in time:
The free parameters are the N+1 coefficients p = [p0, p1, …, pN]T. We do the same for y(t), z(t), and ψ(t) (yaw) — independently, since the cost function decouples over these flat outputs (more on that in Chapter 6).
The Euler-Lagrange equation tells us: minimizing ∫(P(r))2dt without any endpoint constraints requires a polynomial of degree N = 2r−1. For minimum-snap (r=4), this gives N = 7. The general rule: N = 2r−1 for the single-segment, unconstrained case.
In practice we often use higher-degree polynomials (e.g., N=9 is common) to get more degrees of freedom for satisfying continuity constraints at many waypoints.
Differentiating r times:
The product ∏(i−m) for m=0..r−1 is just the falling factorial: i(i−1)(i−2)…(i−r+1). For example, the 4th derivative of p7t7 is 7×6×5×4 · p7 · t3 = 840 p7 t3.
For M waypoints (producing M−1 segments), we define one polynomial Pi(t) per segment. The total trajectory is piecewise:
The cost decomposes over segments: J = J1 + J2 + … + JM−1. Each Ji is an integral of the squared r-th derivative of Pi. This sum structure is key to building the block-diagonal QP cost matrix (Chapter 4).
We've established that higher r gives smoother trajectories. So why not minimize the 6th or 8th derivative? The answer is specific to quadrotor dynamics, and it comes from differential flatness.
From L3 (Quadrotor Dynamics), the total thrust f and the moment vector τ are the actual control inputs. These are produced by combinations of rotor spin speeds. When you work through the Newton-Euler equations for a quadrotor, the total thrust is approximately:
But the attitude (roll and pitch angles) needed to produce a desired translational acceleration involves more derivatives. When you compute the desired body z-axis zB,d from the desired acceleration vector, and then differentiate it to get the desired angular velocity, you find that:
The motor torques needed to achieve a desired angular acceleration are τ = Iα. So snap is the proximal cause of motor torque demand. Minimizing snap minimizes peak motor torque, which keeps the trajectory within the actuator limits of the quadrotor.
For position components x, y, z (each optimized separately), the cost is:
Note: yaw ψ only requires minimizing the 2nd derivative (angular acceleration) because yaw torque maps directly to the 2nd derivative of yaw, not the 4th. This is why the yaw cost uses r=2.
A segment flies from x=0 to x=3 in T seconds. Sliders control the approach/departure speeds. The canvas shows position, jerk, and snap profiles. High snap = motor torque spike.
Now for the key insight that makes trajectory optimization tractable: the minimum-snap problem is a Quadratic Program (QP). This means the cost is quadratic in the unknown parameters (polynomial coefficients) and the constraints are linear. QPs have globally optimal solutions and can be solved in milliseconds.
Let p = [p0, p1, …, pN]T be the coefficient vector. The r-th derivative of P(t) is a linear function of p (see the falling factorial formula from Ch. 2). So P(r)(t)2 is a quadratic function of p. Integrating from 0 to T:
where Q is an (N+1)×(N+1) positive semi-definite matrix. The elements of Q are computed from:
The first r rows and columns are zero because differentiating r times kills all terms with degree < r. The remaining block (indices r through N) forms a dense symmetric positive semi-definite submatrix.
Polynomial: x(t) = p3t3 + p2t2 + p1t + p0. Second derivative: x''(t) = 6p3t + 2p2.
Packing into pTQp with p = [p0, p1, p2, p3]T:
The first 2×2 block is zero (as expected, since r=2 kills p0 and p1). With T=1: Q22=4, Q23=Q32=6, Q33=12.
Subject to constraints Ap = b (waypoint + continuity, derived in Ch. 5):
For multiple segments, Q is block-diagonal (each segment contributes one block) and p stacks all segment coefficient vectors. This QP can be solved via the KKT conditions or any standard QP solver.
Color-mapped Q matrix for polynomial degree N and derivative order r. Hover a cell to see Qlk. The first r rows/cols are zero. The active block grows with N.
The QP cost matrix captures the smoothness objective. Now we need the constraint matrix A and vector b. These encode two things: (1) the trajectory must pass through each waypoint, and (2) it must do so smoothly (matching derivatives across segment boundaries).
For a polynomial P(t) of degree N with coefficient vector p, evaluating at time t0 is linear in p:
The q-th derivative at t0 is similarly linear. Define the row vector:
Then P(q)(t0) = eq(t0) · p. This lets us stack all constraints as a linear system Ap = b.
For waypoint i at time ti, each flat output must equal the waypoint value. For the start of segment i+1 and end of segment i:
These are 2 linear equations per waypoint per flat output. Adding them to A,b is straightforward.
At each internal waypoint, the trajectory and its first r−1 derivatives must match between adjacent segments:
Each of these is also linear in the concatenated coefficient vector p = [p1; p2; …; pM−1]. For r=4, we enforce continuity of position, velocity, acceleration, and jerk at each waypoint — 4 constraints per internal waypoint per flat output.
N=5 (degree-5 polynomial: 6 coefficients). Boundary conditions: position, velocity, acceleration at both endpoints = 6 constraints. Let x(0)=0, x'(0)=0, x''(0)=0 and x(T)=1, x'(T)=0, x''(T)=0 (start and end at rest).
The constraint system Ap=b becomes:
python import numpy as np T = 1.0 # segment duration # Rows: [P(0), P'(0), P''(0), P(T), P'(T), P''(T)] # Cols: [p0, p1, p2, p3, p4, p5] A = np.array([ [1, 0, 0, 0, 0, 0 ], # P(0) = p0 [0, 1, 0, 0, 0, 0 ], # P'(0) = p1 [0, 0, 2, 0, 0, 0 ], # P''(0) = 2p2 [1, T, T**2, T**3, T**4, T**5], # P(T) [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], # P'(T) [0, 0, 2, 6*T, 12*T**2, 20*T**3], # P''(T) ]) b = np.array([0, 0, 0, 1, 0, 0]) p = np.linalg.solve(A, b) # Result: p = [0, 0, 0, 10, -15, 6] # x(t) = 10t^3 - 15t^4 + 6t^5 (classic minimum-jerk)
This is the famous minimum-jerk trajectory: x(t) = 10t3 − 15t4 + 6t5. Verify: x(0)=0 ✓, x(1)=10−15+6=1 ✓, x'(0)=0 ✓, x'(1)=30−60+30=0 ✓, x''(0)=0 ✓, x''(1)=60−180+150=30 ≠ 0 ... wait, let’s check x''(1) = 60−180+150 = 30... that's not 0. Actually for pure minimum-jerk (no acceleration constraint), there are only 4 boundary conditions (pos+vel at each endpoint), giving a degree-3 polynomial: the 10t³−15t⁴+6t⁵ form enforces zero acceleration as well (all 6 conditions). Let’s verify x''(1): P''(t)=60t−180t²+150t³; P''(1)=60−180+150=30? Hmm: 60t is 0+0+2×0+(6t·p3)+(12t²·p4)+(20t³·p5). Let me be precise: x''(t) = 6p3 + 12p4·t + 20p5·t² = 60t−180t²+120t³? Check: p3=10: 6×10=60; p4=−15: 12×(−15)t=−180t; p5=6: 20×6t²=120t². x''(1)=60−180+120=0 ✓. (The coefficient is 120 not 150 — a typo above corrected here.)
We've been treating the trajectory optimization as a problem over position x(t). But the quadrotor has 12 states (position, velocity, rotation, angular velocity) and 4 inputs (rotor speeds). Why can we get away with planning only in x(t), y(t), z(t), ψ(t)?
The answer is differential flatness. A system is differentially flat if there exists a set of flat outputs σ(t) such that the full state and all inputs can be written as smooth functions of σ and its derivatives — without integrating any differential equation.
For a quadrotor, the flat outputs are σ(t) = [x, y, z, ψ]T — three position coordinates plus yaw angle. This means: given any smooth trajectory x(t), y(t), z(t), ψ(t), we can compute exactly what rotor speeds are needed at every instant, without solving any ODEs.
Given x(t), y(t), z(t), ψ(t), compute (step by step):
Every step is an algebraic (no ODE integration) function of x(t) and its derivatives up to 4th order. So a minimum-snap polynomial trajectory in the flat outputs automatically specifies the full quadrotor state and inputs at every time instant.
One free parameter we haven't addressed: how long should each segment take? If Ti is too large, the drone flies very slowly and wastes time. If Ti is too small, the required accelerations saturate the motors.
The cost J = pTQp depends on T through the Q matrix entries (recall Qlk ∝ Tl+k−2r+1). A smaller T "compresses" the trajectory into less time, increasing all derivatives. The optimal timing balances the smoothness cost against a time penalty.
where λ is a weight trading off smoothness vs. speed. A larger λ makes the drone prefer shorter segment times (faster flight). A smaller λ gives more room for smooth transitions.
Because Jtotal is differentiable with respect to Tk, we can optimize the segment times using gradient descent. This creates an outer loop: for a fixed set of Tk, solve the QP to get optimal p; then update Tk via ∇Jtotal/∇Tk. Iterate until convergence.
A practical shortcut: start with Tk proportional to the Euclidean distance between waypoints k and k+1. This heuristic works well and only needs gradient refinement for tight constraints.
Three waypoints. Drag the time allocation slider to compress or expand the second segment time. Watch the peak acceleration change (red dashed line = motor limit).
Every concept in this lesson comes together here. Click on the canvas to place waypoints, drag to reposition them, and watch the minimum-snap trajectory update in real time. The derivative profiles below show position, velocity, acceleration, and jerk — with continuity visible at segment joins.
Top: 2D space with waypoints (white dots) and the minimum-snap curve (teal). Bottom: derivative profile over time (toggle which derivative to display). Color of derivative curve = orange (feasible) or red (exceeds limit).
| Concept | Key formula / fact |
|---|---|
| Smoothness = derivative minimization | Jr = ∫(x(r))2dt — r=1 vel, r=2 accel, r=3 jerk, r=4 snap |
| Min-snap matters for quadrotors | Snap ∝ angular acceleration ∝ motor torque demand |
| Polynomial order (Euler-Lagrange) | N = 2r−1 (single segment); N=7 for r=4 |
| QP cost matrix | J = pTQp, Qlk = [∏(l−m)(k−m)] · Tl+k−2r+1/(l+k−2r+1), first r rows/cols = 0 |
| Constraint matrix | Ap = b; rows from eq(t0) · p = value; continuity: r constraints per internal waypoint |
| Multi-segment | Q block-diagonal, p stacked; M segments × (N+1) coefficients |
| Differential flatness | Flat outputs σ = [x,y,z,ψ]: full state + inputs are algebraic functions of σ and derivatives to order 4 |
| Time allocation | Jtotal = Jr + λ∑Tk; gradient descent on Tk; snap scales as 1/T4 |
| Feasibility check | After solve: verify max thrust, max torque ≤ motor limits everywhere on trajectory |
VNAV L3: Control — How the trajectory is tracked. The geometric controller consumes Rd(t), ωd(t) computed from the flat output. The full differential-flatness pipeline connects directly to the cascaded control loop from L3.
VNAV L5: Image Formation — The perception side begins next: how the camera produces the measurements that feed back into the planning loop. Camera Models & Image Formation.
MDP / RL: Markov Decision Processes provide an alternative planning framework where trajectories emerge from a value function rather than explicit optimization.
Minimum-jerk in robot arms: Inverse Kinematics uses the same polynomial-trajectory philosophy for joint-space trajectory generation.
"Optimization is the language in which we write intent. The QP doesn't know what a drone is — it only sees a quadratic objective and linear constraints. The engineering insight is choosing the objective and constraints that translate intent into a feasible flight."
— paraphrasing Mellinger & Kumar, ICRA 2011