Polynomial Trajectory Optimization for Quadrotors

Our goal is to help drone perform a flight from point A to point B. Whole flight is usually broken into waypoints in order to cover some ground and define corners that we need to take into account. We cannot just connect the dots and call it a day. We need to obey physics and the natural way to go about is to generate a smooth curve called trajectory. Taking corner smoothly is quite important; we cannot just abandon velocity that goes through one waypoint to the other. We need to control velocity since jerk requires an instantaneous change in motors thrust. The problem is the motors can’t actually do that, so the trajectory becomes physically unrealizable.

We need to find a right cost function for quadrotors. Since they don’t care about position, velocity, acceleration or jerk - we need to find a higher derivative. Here comes the snap. As we prove in the next sections minimising snap is minimising motor effort directly - torques the motors need to produce.

Which derivative should you minimise?

All four trajectories connect the same start and goal in the same time but only one of them is physically realizable. Watch the snap bars at the bottom to see why.

Minimising snap means the motors change their torque output as little as physically possible while still completing the trajectory.

Where the polynomial order comes from

The right polynomial order isn’t chosen by intuition — it’s derived. The Euler-Lagrange equation applied to the cost J = ∫ (P⁽ʳ⁾)² dt reduces to a single ODE:

d²ʳ/dt²ʳ P(t) = 0

The general solution is a polynomial of degree N = 2r − 1. For minimum snap (r=4), that’s a 7th-order polynomial — not 5th, not 6th.

Each segment needs 2r boundary conditions to fully determine it: position, velocity, acceleration, and jerk at both endpoints. Exactly 8 constraints for 8 coefficients. No over or under-determination possible.

Scaling to multiple waypoints and further minimising of cost function

For M waypoints we need to solve one linear system, instead of M - 1 segments. Why? Coupling that is given by constraints causes that, derivatives throughout segments must match at junctions. Break that coupling and you’ll get velocity that at the corners is jerky or even physically impossible.

Even given constraints we can optimize further, but we need to get some freedom. Some derivatives need to be free. This way we can move from polynomial coefficients space to endpoint derivative space.

This doesn’t stop us to optimise further. So far time was fixed between the waypoints. We planned the mission beforehand, assumed it’ll take some defined time T, but it doesn’t have to be the case. We can use for that good, old gradient descent and calculate trajectories that manifest the trade-off between smoothness and time.

Gradient descent on the time-augmented cost J = J_r + Σ c_k T_k finds the shortest feasible trajectory.

The gap between waypoints and trajectories

Here’s the part that surprised me but is quite obvious if you think for a bit: obstacle-free waypoints don’t guarantee an obstacle-free trajectory.

A polynomial trajectory is a curve. It passes through infinitely many points between waypoints. The solver only sees boundary conditions (position, velocity, acceleration at endpoints). Everything in between is determined by smoothness alone, which has no concept of walls.

The fix is iterative: detect the first collision, sample a new obstacle-free waypoint from the path planner at that location, insert it, and re-solve.

Press Play to watch the replanning loop. Each iteration adds one waypoint (gold diamond) and recomputes the trajectory. Faint blue lines show earlier failed attempts. The final collision-free path is drawn in green.

This is just toy midpoint heuristic as “path planner” in 2D. In real systems there are many useful algorithms like: RRT* or A*. Usually LiDAR is used for real time updates of the 3D occupancy maps.

The full stack

Here comes the full architectural insight. We combine path planning which is topology that asks: “is there a route?” and trajectory optimization which is part of geometry that asks: “what’s the smoothest curve?“. These are two different problems, but our demo iterates between them:

• Path planner’s job: provide obstacle-free intermediate points
• Trajectory optimizer’s job: just fit the smoothest curve through all of those points