M5-T10: Waypoint navigation

Learning objectives

After this lesson, you should be able to:

• Define a leg, cross-track error and along-track distance from two waypoints.
• Choose acceptance radius and speed at waypoints from vehicle dynamics and mission requirements.
• Implement a carrot (look-ahead) guidance law and explain its tuning parameter.
• List the safety conditions a waypoint mission must monitor.

Conceptual explanation

Established Line-of-sight and carrot guidance are standard textbook methods. Specific autopilot behaviour (acceptance radius parameters, corner blending, speed limits) is implementation-specific: read the documentation of your autopilot.

A waypoint mission is an ordered list of positions (and optionally altitude, speed, yaw and hold time). Between waypoints $W_{k}$ and $W_{k+1}$ the vehicle flies a leg. Two ideas matter:

• Guidance: what point or velocity to command at each instant. Flying directly at the next waypoint (pure pursuit of the waypoint) is simple, but a gust that pushes the vehicle off the line leaves it on a curved path. Flying toward a point on the leg a look-ahead distance ahead of the vehicle’s projection (carrot or line-of-sight) holds the line.
• Acceptance: the condition that advances to the next waypoint. A radius that is too small means the vehicle circles or stalls; too large and corners are cut.

Engineering intuition

Imagine leading a dog with a treat held ahead of it along a rope. A short rope (small look-ahead) makes the dog hug the line but jerk around; a long rope makes it smooth but lets it cut corners. The look-ahead distance is the single most important tuning knob, and it should scale with speed: at 10 m/s a 2 m look-ahead is asking for oscillation, because the vehicle crosses it in 0.2 s.

Acceptance is the same trade-off in time. The vehicle is a physical object with lag; it cannot turn instantly. If you require it to be within 0.2 m of a waypoint before turning, it must decelerate to nearly zero first. If you accept within 3 m, it turns early and flies a rounded corner at speed. Neither is wrong: the mission requirement (photograph the target? pass a gate? survey a field?) decides.

Mathematical formulation

For a leg from A to $B$ with unit direction $u=(B-A)/\Vert B-A\Vert$ and vehicle position $p$, define the projection and the errors:

A velocity command toward the carrot with speed $v$ is $v(p_{\text{carrot}}-p)/\Vert p_{\text{carrot}}-p\Vert$. The geometry gives the cross-track dynamics for small errors: with speed $v$ the cross-track error decays with time constant about L/v, so

Choosing $L=v\,\tau$ for the desired time constant $\tau$ (say 2–4 s for a smooth multirotor, depending on its limits) makes $L$ scale with speed. Corner speed is limited by the available lateral acceleration $a_{\max}$: for a turn of radius $r$, $v\le \sqrt{a_{\max} r}$, and a corner of interior angle $\phi$ blended with acceptance radius $R_{\text{acc}}$ has a radius of roughly $R_{\text{acc}}/\cos (\phi /2)$-type geometry (the exact relation depends on the blending method).

$A$, $B$

leg end points (m).

$L$

look-ahead distance (m).

$e_{\text{ct}}$

cross-track error (m).

$a_{\max}$

maximum lateral acceleration the vehicle can sustain $(m/s^{2})$.

Architecture and data flow

Mission item list (from the ground station or the companion computer) → navigator (current leg, acceptance logic) → position/velocity setpoint → position controller → attitude controller. In PX4 the navigator publishes a position setpoint triplet (previous, current and next) that the position controller uses to blend corners; in ArduPilot the equivalent is the waypoint navigation controller with its own acceptance radius and spline options. The safety monitors (geofence, battery, link and estimator status) sit beside the navigator and can override it with return or land.

Real-world UAV applications

• Survey and mapping missions flown as lawn-mower legs.
• Inspection routes along infrastructure.
• Automated delivery and patrol routes.
• Test routines in SITL: the same mission replayed with different wind and sensor noise.

Implementation example

This runs as shown. A point-mass vehicle with a first-order velocity response (time constant 0.5 s, a stand-in for the inner loops) and a steady crosswind flies a 3-waypoint mission with carrot guidance. A pure carrot law has no wind feedforward, so the crosswind leaves a steady cross-track offset of about $w$ $L$ / $v$ (here 1 × 10 / 5 = 2 m); the test confirms the simulated offset matches that prediction and that the mission completes. Reducing the offset needs integral action on cross-track error or a wind estimate.

import numpy as np

def carrot(p, A, B, L):
    u = (B - A) / np.linalg.norm(B - A); s = u @ (p - A)
    n = np.array([-u[1], u[0]]); e_ct = n @ (p - A)
    return A + (s + L) * u, s, e_ct, np.linalg.norm(B - A)

W = [np.array(w, float) for w in [(0, 0), (60, 0), (60, 40)]]
dt, tau_v, v_cmd, R_acc = 0.05, 0.5, 5.0, 2.0
wind = np.array([0.0, 1.0])                           # 1 m/s steady crosswind pushes the vehicle off the first leg
p = W[0].copy(); v = np.zeros(2); leg = 0; worst = 0.0; t = 0.0
while leg < len(W) - 1 and t < 120:
    A, B = W[leg], W[leg + 1]
    c, s, e, length = carrot(p, A, B, L=v_cmd * 2.0)    # look-ahead = speed x 2 s
    c = A + (B - A) * min(1.0, max(0.0, (c - A) @ (B - A) / length ** 2))   # never aim past the waypoint
    dirn = (c - p) / max(np.linalg.norm(c - p), 1e-6)
    v += (v_cmd * dirn - v) / tau_v * dt                # first-order velocity response
    p += (v + wind) * dt; t += dt
    if t > 8.0 and leg == 0: worst = max(worst, abs(e))     # after the transient
    if np.linalg.norm(B - p) < R_acc: leg += 1
print(f"mission complete: {leg == len(W) - 1}, time {t:.1f} s, worst cross-track {worst:.2f} m on leg 1")
print(f"predicted steady offset w L / v = {1.0 * 10.0 / 5.0:.2f} m")
assert leg == len(W) - 1 and 1.5 < worst < 2.5

Common engineering mistakes

Failure modes

Debugging approach

1. Plot the flown path over the planned legs and the cross-track error against time.
2. Log the acceptance distance at each waypoint switch.
3. Replay the mission in SITL with wind off, then on, to separate guidance faults from estimation faults.
4. Check the frame and altitude reference (relative to home, above mean sea level) for each waypoint.

Practical exercises

1. Change the look-ahead to 0.5 s × speed and 6 s × speed and compare cross-track error and corner cutting.
2. Add a corner speed limit $v=\sqrt{a_{\max} r}$ with $a_{\max}=3\,\mathrm{m}/\mathrm{s}^{2}$ and a radius derived from the acceptance circle.
3. Add integral action on cross-track error (or a wind-velocity feedforward) and show the steady offset going to zero.
4. Write the acceptance logic for a waypoint that requires a 5 s hold and a heading.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• PX4: Mission mode (multicopter)
• ArduPilot Copter: Auto mode
• ArduPilot Copter: Parameters (WPNAV_* waypoint navigation parameters)

What’s next