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

This section develops perception and obstacle avoidance as a concrete embodied AI skill rather than a label. 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.

For Perception, navigation, and obstacle avoidance, 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.

Theory

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 depend on the third and fourth derivatives of position, the natural cost to minimize is the snap, the fourth derivative of position:

$$\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

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 the chapter separates three claims. The conceptual claim explains why the skill should work. The systems claim explains which interface changes. The evidence claim records which same-panel metric would convince a skeptical builder.

For Perception, navigation, and obstacle avoidance, 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.