Section 47.3: Perception, navigation, and obstacle avoidance

"A smooth trajectory is not a luxury; it is the only kind a quadrotor can actually fly."

A Careful Control Loop

Picture a quadrotor threading a doorway at 6 m/s: it has covered another 6 cm in the time it took you to read that clause, and if its obstacle map is even one cycle stale, the door frame it saw a moment ago is no longer where the planner thinks it is. That single unforgiving fact is what turns perception, navigation, and obstacle avoidance into a concrete embodied AI skill rather than a label. By the end you should be able to describe the full aerial perception-navigation-avoidance loop as one inspectable contract: how depth and optical flow become a live obstacle map, how a replanning cycle is tuned to the vehicle's speed envelope, and why clearance margin (not just task completion) is the metric that matters. Figure 47.3A previews this loop end to end, tracing a depth camera through an occupancy voxel map, a local planner, and IMU-plus-visual-odometry fusion that holds position in GPS-denied spaces. The core contract is: observe depth, optical flow, map uncertainty, and free-space estimate, choose safe velocity or waypoint commands under limited sensing, and judge the result with collision rate and clearance margin.

Concretely, each control cycle collapses those three subsystems into a single decision: the obstacle distance field (introduced below) tells the planner how much clearance any candidate trajectory has, the minimum-snap planner (also introduced below) turns the clearance-checked waypoints into a flyable reference, and the perception failure-mode discussion later in this section specifies when the drone should abandon that reference and fall back to a controlled hover instead. Reading the three parts of this section as one runtime decision, rather than three separate topics, is what makes the loop actionable.

Check the earlier frame, control, and model chapters against the exact interface used here: state variables, timing budget, action limits, and evaluation panel.

Figure 47.3.1 maps the aerial perception interface from camera or lidar stream to state estimate, local obstacle model, trajectory candidate, and avoidance command. The important check is whether each edge names frame, latency, and confidence.

Consider a specific case. The Skydio 2 autonomy stack runs six fisheye cameras at 45 fps, maintains a 3D obstacle map in a 10-meter radius sphere around the vehicle, and replans local trajectories at roughly 100 Hz. The tight replan rate exists because at a cruise speed of 6 m/s the drone covers 6 cm between planning cycles. Any slower replan loop lets a half-meter branch appear between two consecutive maps with no time to brake. The EGO-Planner system (Zhou et al., 2020) takes a different tradeoff. It skips explicit map building entirely and optimizes trajectory coefficients directly against gradient information from a distance field, cutting latency to under 10 ms per replan on an onboard CPU. Both choices show that local planning and obstacle avoidance design is inseparable from the vehicle's speed envelope and compute budget. A traditional map-build-then-replan pipeline needs roughly 80 to 150 ms end to end; EGO-Planner's direct gradient approach cuts that to under 10 ms, roughly a 10x latency reduction, which in turn raises the cruise speed at which a fixed braking-distance margin can still be held before impact becomes critical (the exact speed gain depends on sensor range and deceleration authority, so treat the doubling as an illustrative order of magnitude rather than a guaranteed figure). A planner that is too slow for the vehicle's speed is not a planner; it is a post-crash report.

Theory

With the interface and its timing budget fixed, the next question is what kind of path the planner is allowed to hand the vehicle. The answer begins with why smoothness is non-negotiable.

Why does trajectory smoothness matter so much for aerial robots? A quadrotor cannot decouple its attitude from its acceleration: tilting forward is the only way to accelerate forward, so a jerky velocity profile demands rapid attitude changes, which in turn demand large and rapidly changing rotor thrusts. If those demands exceed what the motors can physically deliver, the vehicle leaves the planned path entirely. Smooth trajectories are therefore not an aesthetic choice; they are a physical necessity imposed by the coupling between translation and rotation in underactuated vehicles.

The obstacle distance field \(\mathcal{D}(\mathbf{x})\) assigns to every point in space the distance to the nearest obstacle surface. For a drone replanning at 100 Hz, this representation is essential. A single lookup replaces expensive geometry queries, letting the planner evaluate clearance and compute a repulsive gradient without iterating over raw sensor points. At flight speed, testing against a raw point cloud at every cycle would saturate the onboard CPU before the avoidance loop could close. A Euclidean distance transform over the 3D voxel occupancy grid builds the field. Each occupied voxel seeds the transform, and the result is a dense grid of precomputed distances that supports constant-time lookup and finite-difference gradient estimation at query time.

From waypoints to a flyable path

Once obstacle-free waypoints are chosen, the planner must connect them with a path the vehicle can physically fly. The key structural fact is differential flatness (Mellinger and Kumar, 2011): a quadrotor's full state and the four motor inputs can be written as algebraic functions of four flat outputs and their derivatives,

$$\sigma = [\,x,\; y,\; z,\; \psi\,],$$

the three positions and the yaw angle. This means you can plan entirely in the smooth \((x, y, z, \psi)\) space, and any sufficiently smooth trajectory there maps back to feasible thrust and attitude commands. Roll and pitch are not planned directly; they fall out of the acceleration profile.

Because rotor thrust is proportional to acceleration and the body moments (the torques that rotate the airframe about its own center of mass) depend on the third and fourth derivatives of position, the natural cost to minimize is the snap, the fourth derivative of position:

In other words: minimizing snap is not an arbitrary mathematical convenience, it is the direct algebraic consequence of the differential-flatness fact just above, since penalizing the fourth derivative of position is the same as penalizing how hard the body moments have to swing.

$$\min \int_0^T \left\lVert \frac{d^4 \mathbf p(t)}{dt^4} \right\rVert^2 dt.$$

Minimizing snap minimizes the aggressiveness of the motor commands, so the trajectory stays inside the actuator envelope and the angular accelerations stay small. Each segment between waypoints is represented as a polynomial in \(t\); for a snap objective the minimizing polynomial per segment is degree 7 (eight coefficients), constrained so that position passes through the waypoints and velocity, acceleration, and jerk stay continuous at the joins. The result is a piecewise-polynomial trajectory whose every derivative is well behaved, which is exactly what the cascaded flight controller of Section 47.6 needs as a reference.

Worked Example

Generate a minimum-snap trajectory through three waypoints in one axis. We fit a degree-6 polynomial that passes through positions 0, 2, 1 at times 0, 1, 2 s, with zero velocity and zero acceleration at both ends (a rest-to-rest segment). Minimizing snap with these endpoint constraints yields a unique polynomial, so we can solve the constraint system directly with numpy.linalg.solve rather than running a full quadratic program.

# Minimum-snap polynomial through 3 waypoints (single axis), rest to rest.
import numpy as np

t_wp = np.array([0.0, 1.0, 2.0])   # waypoint times
p_wp = np.array([0.0, 2.0, 1.0])   # waypoint positions
T, deg = 2.0, 6                    # degree-6 poly: 7 coefficients

def basis(t, d):
    """Row of the d-th derivative of [1, t, t^2, ..., t^6] at time t."""
    r = np.zeros(deg + 1)
    for i in range(deg + 1):
        if i >= d:
            coef = 1
            for k in range(d):
                coef *= (i - k)
            r[i] = coef * t ** (i - d)
    return r

A, b = [], []
for tw, pw in zip(t_wp, p_wp):     # pass through every waypoint
    A.append(basis(tw, 0)); b.append(pw)
for tw in (0.0, T):                # zero velocity and acceleration at the ends
    A.append(basis(tw, 1)); b.append(0.0)
    A.append(basis(tw, 2)); b.append(0.0)

c = np.linalg.solve(np.array(A), np.array(b))   # 7 constraints, 7 unknowns
print("polynomial coefficients:", np.round(c, 3))
print("position at waypoints  :", [round(float(basis(t, 0) @ c), 3) for t in t_wp])
print("end velocities         :", round(float(basis(0, 1) @ c), 4),
                                   round(float(basis(T, 1) @ c), 4))

Expected output: coefficients that reproduce the waypoint positions and zero endpoint velocities. The diagnostic field is the waypoint check: if a position is off, the time allocation or the constraint matrix is wrong, and no amount of controller tuning downstream will fix a reference the planner never actually passes through.

Practical Recipe

That single-axis solve is the smallest piece of the loop that can fail visibly; the recipe below scales the same discipline up to the full perception-navigation-avoidance contract so failures stay just as easy to localize.

1. Write the skill contract: observable variables, action interface, metric, allowed recovery actions, and stop conditions.
2. Build the smallest baseline that can fail in an interpretable way.
3. Run the maintained library version with the same inputs, scenarios, and metric code.
4. Add one perturbation aimed at the expected failure: the drone sees the obstacle too late for its braking distance.
5. Save one artifact containing config, seeds, logs, summary metrics, and two representative traces.

Perception, navigation, and obstacle avoidance becomes robust when three claims stay separate: a conceptual claim for why the skill should work, a systems claim for which interface changes, and an evidence claim naming the same-panel metric that would convince a skeptical builder.

Keep flight physics, airspace constraint, battery state, timing, wind, and safety monitor inside the evidence artifact rather than in a post-run explanation.

When Perception, navigation, and obstacle avoidance fails, do not collapse the whole method into one score. Assign the failure to a subsystem, rerun one perturbation that isolates the suspected cause, and keep the trace as a reusable diagnostic case.