M8-T2 — Mission Guidance: Legs, Loiters and Energy

Learning objectives

After this lesson, you should be able to:

• Compute the start of a fillet turn from the corner angle, speed and bank limit.
• Compute the lateral acceleration and bank angle on a loiter in wind, and the smallest loiter radius.
• Write the specific total energy and its balance, and the rates the throttle and the pitch loop control.
• Explain why total energy control protects the speed in a thrust-limited climb.

The intuition

UAV M5-T10: Waypoint navigation handles a list of waypoints for a multirotor that can stop at each, and Navigation M8-T1: Waypoint Missions and Route Ordering orders them. For a vehicle that cannot stop, three questions arise at each stage of a mission that those pages do not answer: how to turn at a corner, how to circle a point, and how to climb without losing speed.

The examples use a small fixed-wing survey vehicle, not Hopper, because Hopper is a rocket that does hover. The same energy bookkeeping does apply to Hopper's descent, where thrust and the pitch of the thrust vector share the work of height and speed, and the next lesson of the module guides that descent.

Turns are arcs, started early

Between two straight legs that meet at an angle, the vehicle cannot turn on the spot. It flies an arc of a radius set by its speed and the bank it can use, $R = V^2/(g\tan\varphi_{\max})$, tangent to both legs. The arc begins before the corner by the fillet distance $d = R\tan(\Delta\chi/2)$. For 20 m/s, a 30° bank and a 90° corner, $R = 70.6$ m and $d = 70.6$ m. A guidance law whose look-ahead is much shorter than this tries to turn tighter than the vehicle can; one that is much longer cuts across the corner.

A loiter is not the same turn in wind

A circle of radius $R$ flown at ground speed $V_g$ needs a lateral acceleration $V_g^2/R$. With no wind this is constant: 4 m/s² for 20 m/s and 100 m, a bank of 22°. In an 8 m/s wind the ground speed of a 20 m/s vehicle is 12 m/s upwind and 28 m/s downwind round the circle, so the acceleration swings from 1.44 to 7.84 m/s² and the bank from 8° to 39°. The loiter must be designed for the downwind point: a 30° bank limit needs a radius of 138 m with the wind, not 71 m. A guidance law for a circle that uses the ground track sees this and holds the radius to a few metres: the simulation holds 100 m to within 2.5 m without wind and 4.9 m with it, though its acceleration swings between 0.85 and 10.8 m/s², larger than the steady value because the radius error and the heading loop add to it.

Energy, not height and speed

Height and speed are two forms of one resource. The specific total energy is $E_s = h + V^2/2g$, and its rate, divided by the speed, is $\dot{E}_s/V = \gamma + \dot{V}/g$: the flight path angle plus the acceleration in units of g. Thrust minus drag, per unit weight, is exactly that: $(T - D)/W = \gamma + \dot{V}/g$. So thrust controls the total energy. The elevator cannot change the energy (it ignores drag, to first order); it only trades speed for height, the balance $\gamma - \dot{V}/g$. A total energy controller assigns thrust to the demanded rate of energy and pitch to the demanded balance.

What it buys at the limit

A 15 kg vehicle with 40 N of thrust and 16 N of drag at 20 m/s can climb at 3.3 m/s at most. A 100 m climb demand needs more thrust than that. If pitch holds altitude, it pitches up to follow the demand and the speed falls to 11.9 m/s, below the 12 m/s stall speed, while the throttle sits at its limit. A total energy controller sees the speed error enter its balance demand with the opposite sign, so it lowers the pitch demand: the speed dips only to 17.6 m/s and the climb is slower, not dangerous.

Thrust sets the total energy rate, pitch sets its distribution between height and speed. When thrust runs out, a controller that keeps speed as the priority climbs slower; one that keeps altitude as the priority stalls.

Definitions

Fillet

An arc of radius R tangent to two legs, replacing the corner; it begins at distance $d = R\tan(\Delta\chi/2)$ before the corner.

Loiter

A circular leg held at a given radius and direction around a center.

Specific total energy $E_s$

Energy per unit weight, $E_s = h + V^2/(2g)$, in metres.

Energy rate and balance

$\dot{E}_s/V = \gamma + \dot{V}/g$ and $B = \gamma - \dot{V}/g$.

Derivation: the energy equation, and the fillet distance

The longitudinal point-mass equations along the flight path.

1. $$m\dot{V} = T - D - mg\sin\gamma$$

   Force balance along the velocity: thrust minus drag minus the component of weight, for small thrust inclination.

2. $$\dot{h} = V\sin\gamma$$

   Kinematics of the climb.

3. $$\dot{E}_s = \dot{h} + V\dot{V}/g = V\left(\sin\gamma + \dot{V}/g\right) = \dfrac{(T - D)\,V}{W}$$

   Differentiate the specific energy and substitute: the gravity terms cancel. The energy rate is the specific excess power, thrust minus drag times speed per unit weight.

4. $$\dfrac{T - D}{W} = \gamma + \dfrac{\dot{V}}{g}\quad(\sin\gamma \approx \gamma)$$

   Divide by V. Thrust alone changes the sum of the flight path angle and the acceleration, so the throttle controls total energy.

5. $$B = \gamma - \dot{V}/g$$

   The balance is the difference of the same two terms: the elevator can change γ and V̇ in opposite senses without changing their sum.

Why it works at the limit

1. $$\dot{S}_D^* = \gamma_h - \dfrac{V_c - V}{g\,\tau_v}$$

   The demanded balance contains the speed error with a negative sign: speed too low ($V_c > V$) asks for a lower balance, that is, a lower flight path angle for a given acceleration.

2. $$T = T_{\max} \;\Rightarrow\; \gamma + \dot{V}/g = (T_{\max} - D)/W$$

   At the thrust limit the sum is fixed, so a pitch loop that raises γ can only do so by taking acceleration from the speed: the speed falls.

3. $$\text{pitch holds altitude} \Rightarrow \gamma = \gamma_h\ \text{(too large)} \Rightarrow \dot{V} = g\bigl[(T_{\max} - D)/W - \gamma_h\bigr] < 0$$

   With the demand fixed by the altitude error, the speed decays at the rate the thrust shortfall implies, until the stall. Total energy control reduces γ as the speed falls.

The fillet

1. $$d = R\tan(\Delta\chi/2)$$

   Two tangent lines at a turn angle $\Delta\chi$ meet a circle of radius R tangent to both at a distance d from the corner along each line; the half-angle at the corner is $(\pi - \Delta\chi)/2$ and the tangent length is $R/\tan((\pi-\Delta\chi)/2) = R\tan(\Delta\chi/2)$.

2. $$R = V^2/(g\tan\varphi_{\max})$$

   A coordinated turn at bank $\varphi$ has lateral acceleration $g\tan\varphi = V^2/R$.

Thrust minus drag per unit weight is the flight path angle plus the acceleration in g: the throttle sets that sum, and the pitch decides how it is split between climb and speed. When the throttle saturates, the sum is fixed and the controller must decide which of height and speed gives way.

A worked example

A 100 m climb of a 15 kg vehicle at 20 m/s (drag $D = 0.04\,V^2$, thrust limit 40 N), and a loiter.

1. Loiter without wind. At 20 m/s and 100 m radius: 4.00 m/s², bank 22.2°. With an 8 m/s wind: 1.44 m/s² and 8.4° at the upwind point (12 m/s), 7.84 m/s² and 38.6° at the downwind point (28 m/s).
2. Smallest radius. With a 30° bank limit: 70.6 m without wind and 138.4 m with a downwind ground speed of 28 m/s; with a 45° limit, 40.8 m and 79.9 m.
3. Simulated loiter. The circle-following law holds the mean radius at 102.5 m (no wind) and 102.3 m (8 m/s) with deviations 2.5 and 4.9 m; the lateral acceleration is a constant 3.90 m/s² in still air and swings between 0.85 and 10.78 m/s² in wind.
4. Climb capability. With full thrust at 20 m/s the vehicle can climb at 3.26 m/s (9.3°). The 100 m demand needs a flight path angle near 14° and 53 N, more than the 40 N available.
5. Total energy control. The climb takes 29.2 s to reach 90 m, the lowest speed is 17.61 m/s, and the vehicle ends at 99.98 m and 20.00 m/s.
6. Pitch for altitude, throttle for speed. The climb is 2 s faster (27.2 s) but the speed falls to 11.93 m/s, below the 12 m/s stall speed, before recovering to 20 m/s at 100 m.

import math

G0, D2R = 9.81, math.pi / 180
V = 20.0
print('Loiter on a circle of radius R: the lateral acceleration is Vg^2 / R and the bank angle atan(Vg^2 / (g R))')
R = 100.0
for name, vg in (('no wind', 20.0), ('upwind point, 8 m/s wind', 12.0), ('downwind point, 8 m/s wind', 28.0)):
    a = vg * vg / R
    print(f'  {name:28s} ground speed {vg:4.1f} m/s: lateral acceleration {a:5.2f} m/s2, bank {math.degrees(math.atan(a / G0)):5.1f} deg')
for phi in (30.0, 45.0):
    print(f'  smallest radius at a bank limit of {phi:.0f} deg: no wind {V ** 2 / (G0 * math.tan(phi * D2R)):6.1f} m, downwind speed 28 m/s {28.0 ** 2 / (G0 * math.tan(phi * D2R)):6.1f} m')

def loiter_run(wind, R=100.0, k=0.04, T=400.0, dt=0.02):
    """Circle centred on the origin, counter-clockwise. Course command = tangent + atan(k (rho - R)) pointing inward when outside; heading loop psi' = 3 (course - psi)."""
    x, y, psi = -250.0, 60.0, 0.0
    peak_a = 0.0
    tail = []
    for i in range(int(T / dt)):
        gx, gy = V * math.cos(psi) + wind, V * math.sin(psi)
        rho = math.hypot(x, y)
        tang = math.atan2(y, x) + math.pi / 2
        crs = tang + math.atan(k * (rho - R))          # outside the circle: turn left of the counter-clockwise tangent, inward
        chi = math.atan2(gy, gx)
        err = (crs - chi + math.pi) % (2 * math.pi) - math.pi
        psidot = 2.0 * err
        psi += psidot * dt
        x += gx * dt; y += gy * dt
        a = math.hypot(gx, gy) * abs(psidot)
        if i * dt > T - 100:
            tail.append((rho, a))
    rho_mean = sum(r for r, a in tail) / len(tail)
    rho_dev = max(abs(r - R) for r, a in tail)
    return rho_mean, rho_dev, max(a for r, a in tail), min(a for r, a in tail)
for wind in (0.0, 8.0):
    rm, rd, amax, amin = loiter_run(wind)
    print(f'simulated loiter with wind {wind:3.0f} m/s: mean radius {rm:6.2f} m, radius deviation {rd:5.2f} m, lateral acceleration between {amin:5.2f} and {amax:5.2f} m/s2')

# total energy control against pitch-for-altitude / throttle-for-speed on a thrust-limited climb
m, kd = 15.0, 0.04                                   # mass (kg), drag D = kd V^2 (N s^2/m^2)
Tmax, Tmin = 40.0, 0.0
def D(v): return kd * v * v
def run(mode, hc=100.0, Vc=20.0, T=80.0, dt=0.02):
    h, v, gam = 0.0, 20.0, 0.0
    rows = []
    vmin = v
    for i in range(int(T / dt)):
        t = i * dt
        if mode == 'tecs':
            gE = max(-0.25, min(0.25, (hc - h) / (v * 8.0)))
            vdot_cmd = (Vc - v) / 4.0
            SE = gE + vdot_cmd / G0
            SD = gE - vdot_cmd / G0
            thr = D(v) + m * G0 * SE
            SDact = gam - (rows[-1][3] if rows else 0.0) / G0
            gcmd = SD + 1.0 * (SD - SDact)
        else:
            gcmd = max(-0.25, min(0.25, (hc - h) / (v * 8.0)))
            thr = D(Vc) + 4.0 * (Vc - v) * m * 0.5
        thr = max(Tmin, min(Tmax, thr))
        vdot = (thr - D(v)) / m - G0 * gam
        gam += (max(-0.4, min(0.4, gcmd)) - gam) / 1.0 * dt
        h += v * gam * dt
        v += vdot * dt
        vmin = min(vmin, v)
        rows.append((t, h, v, vdot, thr))
    t90 = next((r[0] for r in rows if r[1] >= 0.9 * hc), float('nan'))
    return rows, vmin, t90
print('climb 100 m at 20 m/s; thrust needed for the climb exceeds the 40 N available (drag 16 N at 20 m/s, 15 kg)')
print(f'  steady climb rate possible with full thrust at 20 m/s: {((Tmax - D(20.0)) / (m * G0)) * 20.0:.2f} m/s (flight path angle {(Tmax - D(20.0)) / (m * G0) / D2R:.1f} deg)')
for mode, name in (('tecs', 'total energy control (throttle: energy, pitch: balance)'), ('conv', 'pitch holds altitude, throttle holds speed')):
    rows, vmin, t90 = run(mode)
    h_end = rows[-1][1]; v_end = rows[-1][2]
    print(f'  {name:55s}: 90% of the climb in {t90:5.1f} s, lowest speed {vmin:5.2f} m/s, final altitude {h_end:6.2f} m, final speed {v_end:5.2f} m/s')
print('   speed floor for this vehicle: assume a stall speed of 12 m/s')

Loiter on a circle of radius R: the lateral acceleration is Vg^2 / R and the bank angle atan(Vg^2 / (g R))
  no wind                      ground speed 20.0 m/s: lateral acceleration  4.00 m/s2, bank  22.2 deg
  upwind point, 8 m/s wind     ground speed 12.0 m/s: lateral acceleration  1.44 m/s2, bank   8.4 deg
  downwind point, 8 m/s wind   ground speed 28.0 m/s: lateral acceleration  7.84 m/s2, bank  38.6 deg
  smallest radius at a bank limit of 30 deg: no wind   70.6 m, downwind speed 28 m/s  138.4 m
  smallest radius at a bank limit of 45 deg: no wind   40.8 m, downwind speed 28 m/s   79.9 m
simulated loiter with wind   0 m/s: mean radius 102.50 m, radius deviation  2.50 m, lateral acceleration between  3.90 and  3.90 m/s2
simulated loiter with wind   8 m/s: mean radius 102.32 m, radius deviation  4.85 m, lateral acceleration between  0.85 and 10.78 m/s2
climb 100 m at 20 m/s; thrust needed for the climb exceeds the 40 N available (drag 16 N at 20 m/s, 15 kg)
  steady climb rate possible with full thrust at 20 m/s: 3.26 m/s (flight path angle 9.3 deg)
  total energy control (throttle: energy, pitch: balance): 90% of the climb in  29.2 s, lowest speed 17.61 m/s, final altitude  99.98 m, final speed 20.00 m/s
  pitch holds altitude, throttle holds speed             : 90% of the climb in  27.2 s, lowest speed 11.93 m/s, final altitude 100.00 m, final speed 20.00 m/s
   speed floor for this vehicle: assume a stall speed of 12 m/s

In robotics

The three parts of this lesson are the three items of a mission plan.

• Turn anticipation. Autopilots begin a turn before the waypoint using the fillet distance (or an equivalent acceptance radius from the turn radius), so that the vehicle is on the next leg tangent to it. The distance grows with speed squared, so a fast vehicle starts turning hundreds of metres early.
• Loiter and orbit modes. A loiter is used to wait, to survey a point, or to hold above a landing site. In wind the bank is set by the downwind point; a flight plan that picks the radius from the still-air bank will overrun the limit exactly where the vehicle is fastest.
• Total energy in practice. Common open-source fixed-wing autopilots implement a version of total energy control (TECS) for exactly the saturation case above, and add a rule that lowers the pitch demand when the throttle is at its limit. The same idea underlies speed-priority climb in airliners.
• Not only aircraft. Any vehicle with a coupled pair of states and one shared actuator, a boat's speed and heading with its engine, a rocket's altitude and speed with its throttle, has an equivalent split.
• Altitude capture. A level-off is the same controller with the energy demand falling to zero, the energy rate demand and the balance converging together; the damped approach of the first loop is the energy loop with its own time constant.

The path-following law that flies each leg is M8-T1: Path-Following Guidance for Vehicles That Cannot Stop. The lab M8-L1: Hold the Survey Line in Wind puts a survey line through a crosswind. The same lateral accelerations must be within the limits of the allocation of M5-T6: Control Allocation and the saturation handling of M3-T7: Digital Control: Sampling, Delay and Anti-Windup.

Common misconceptions

Key takeaways

1. A fillet has radius $R = V^2/(g\tan\varphi_{\max})$ and starts $R\tan(\Delta\chi/2)$ before the corner: 70.6 m for 20 m/s, 30° bank and 90°.
2. A loiter in wind of speed w has ground speed from V − w to V + w, lateral acceleration $V_g^2/R$: 1.44 to 7.84 m/s² at R = 100 m and w = 8 m/s.
3. The loiter radius is set by the downwind point: 138 m with a 30° bank limit, against 71 m in still air.
4. Thrust minus drag per unit weight equals $\gamma + \dot{V}/g$: the throttle controls total energy, the pitch its balance $\gamma - \dot{V}/g$.
5. In a thrust-limited climb, total energy control dips to 17.6 m/s; pitch-for-altitude drops to 11.9 m/s, below the stall speed.
6. At the thrust limit the controller must choose between speed and height: total energy control chooses speed.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• A. A. Lambregts, "Vertical flight path and speed control autopilot design using total energy principles", AIAA Paper 83-2239, 1983, the origin of total energy control. Paid (AIAA).
• R. W. Beard and T. W. McLain, Small Unmanned Aircraft: Theory and Practice, Princeton University Press, 2012, the chapters on path management and on fillets. Paid.
• ArduPilot documentation, TECS speed and height tuning guide , a working description of the total energy controller. Free to read; licence CC BY-SA 3.0.
• PX4 documentation, fixed-wing position and altitude modes , for how a flight stack exposes these modes. Free to read; licence CC BY 4.0.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M8-T1: Path-Following Guidance for Vehicles That Cannot Stop

  Derive the cross-track dynamics of nonlinear look-ahead guidance and of a vector field, choose the look-ahead for a damping ratio, and predict behaviour in wind.

• Lab
  M8-L1: Hold the Survey Line in Wind

  A survey aircraft overshoots every turn in a crosswind. Implement look-ahead path guidance, choose the look-ahead from the cross-track dynamics, and hold the line within 2 m.

Other courses on this site treat neighbouring topics from their own angle:

• Navigation M5-T4: Pure Pursuit
• Navigation M5-T5: Stanley and Cross-Track Control
• Navigation M4-T5: Dubins and Reeds-Shepp Curves
• Navigation M8-T1: Waypoint Missions and Route Ordering
• Navigation M6-T4: Time to Collision and Predicting Moving Obstacles
• Navigation M8-T6: Docking, Charging and Fleet Operations
• UAV M5-T10: Waypoint navigation
• UAV M5-T14: Trajectory tracking
• UAV M5-T15: Guidance and control integration

This course leaves the following to the pages that already teach them:

• Path planning: graph search, sampling-based and kinodynamic planners
• Pure pursuit and Stanley as path trackers for wheeled robots
• Obstacle avoidance and local planning
• Waypoint acceptance radii and mission items of a multirotor