M5-T6 — Control Allocation

Learning objectives

After this lesson, you should be able to:

• Write the allocation problem for a redundant set of effectors as a linear map with limits.
• Compute the minimum-norm solution with the pseudoinverse and test it against the limits.
• Solve the bounded problem and use weights to set priorities when the demand is infeasible.
• Describe the attainable moment set and how a failed effector changes it.

The intuition

UAV M1-T6: Roll, pitch, yaw and thrust generation turned four desired quantities into four motor speeds by a fixed square matrix. With as many effectors as demands, nothing needs to be chosen. The Linear Algebra track's pseudoinverse and null-space control pages give the algebra for the case with more effectors than demands; this lesson adds the limits, which are what make the problem hard.

More effectors than demands

Take four thrusters at the corners of a square, each producing 0 to 600 N, and command the total thrust $F$ and the roll and pitch moments $M_x,\ M_y$: three numbers, four unknowns. There is a one-dimensional family of solutions. The pseudoinverse picks the one with the smallest total squared thrust, which is balanced and simple: for $F = 1000$, $M_x = 60$, $M_y = -40$ it gives (260, 300, 240, 200) N, all inside the limits.

When a limit bites

The pseudoinverse knows nothing about the limits. For a large demand it returns a thrust above 600 N or below zero, and clipping it to the limits changes the force and moment in an uncontrolled way. The principled version is a constrained least-squares problem: the closest achievable force and moment, with every effector in range. The weights say what to sacrifice first.

After a failure

If thruster 1 stops, three effectors remain for three demands and the solution is unique, with no freedom left. For the same command it needs thruster 3 to push −20 N, which does not exist. The allocator returns (0, 550, 0, 450) N, which produces the thrust but moments of (50, −50) instead of (60, −40). More revealing is the set of what can be done at all: at 1000 N the four thrusters give up to 500 N m about each axis in both directions; with one lost, a positive roll or positive pitch cannot be produced at all without producing the other, and the vehicle must accept coupling or give up thrust.

Allocation chooses among many ways to do the same thing, within limits. After a failure the question is no longer which way, but whether the demand can be met at all, and what to give up.

Definitions

Effectiveness matrix $\mathbf{B}$

The matrix that maps the effector vector $\mathbf{u}$ to the generalized force $\mathbf{v}$: $\mathbf{v} = \mathbf{B}\mathbf{u}$. Here $\mathbf{B}$ is 3×4, so it has a one-dimensional null space.

Attainable set

$\{\mathbf{B}\mathbf{u} : \mathbf{u}_{\min} \le \mathbf{u} \le \mathbf{u}_{\max}\}$: the demands that can be met at all with the limits.

Pseudoinverse solution

$\mathbf{u} = \mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}}(\mathbf{B}\mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}})^{-1}\mathbf{v}$: the smallest weighted $\mathbf{u}^{\mathsf{T}}\mathbf{W}\mathbf{u}$ that meets the demand exactly, ignoring the limits.

Bounded least squares

The minimization of $\|\mathbf{W}_v(\mathbf{B}\mathbf{u} - \mathbf{v})\|^2$ over the box of limits.

Derivation: the pseudoinverse, and the attainable set

The minimum-norm solution of $\mathbf{B}\mathbf{u} = \mathbf{v}$ and the structure of the attainable set.

1. $$\min\ \tfrac{1}{2}\mathbf{u}^{\mathsf{T}}\mathbf{W}\mathbf{u} \quad \text{subject to}\quad \mathbf{B}\mathbf{u} = \mathbf{v}$$

   Of the infinite family of exact solutions pick the one with the smallest weighted squared effort.

2. $$\mathbf{W}\mathbf{u} = \mathbf{B}^{\mathsf{T}}\boldsymbol{\lambda}$$

   Set the gradient of the Lagrangian $\tfrac{1}{2}\mathbf{u}^{\mathsf{T}}\mathbf{W}\mathbf{u} - \boldsymbol{\lambda}^{\mathsf{T}}(\mathbf{B}\mathbf{u} - \mathbf{v})$ to zero.

3. $$\mathbf{u} = \mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}}\boldsymbol{\lambda}, \qquad \mathbf{B}\mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}}\boldsymbol{\lambda} = \mathbf{v}$$

   Solve for $\mathbf{u}$ and substitute into the constraint to find the multipliers.

4. $$\mathbf{u} = \mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}}(\mathbf{B}\mathbf{W}^{-1}\mathbf{B}^{\mathsf{T}})^{-1}\mathbf{v}$$

   The weighted pseudoinverse; for $\mathbf{W} = \mathbf{I}$ it is the Moore-Penrose solution. It exists when the rows of B are independent.

The attainable set and a failure

1. $$\mathcal{V} = \mathbf{B}\,[\mathbf{u}_{\min}, \mathbf{u}_{\max}]$$

   The image of a box under a linear map is a convex polytope in the space of demands.

2. $$\max_{\mathbf{u}}\ \mathbf{w}^{\mathsf{T}}\mathbf{B}\mathbf{u} = \sum_j \max\bigl(w^{\mathsf{T}}\mathbf{b}_j\,u_{\min,j},\ w^{\mathsf{T}}\mathbf{b}_j\,u_{\max,j}\bigr)$$

   The extent of the polytope in any direction $\mathbf{w}$ is a sum over the effectors, each at the limit that suits it: this is how the region in the figure is drawn (with the fixed total thrust as one more constraint).

3. $$u_{\max,1} \to 0$$

   A failed effector has its upper limit set to its failed value. Every term of the sum for it becomes a fixed contribution, the polytope shrinks, and it may lose the origin from its interior in some axes.

Allocation is a linear map from effectors to demands, plus limits. The pseudoinverse solves the unconstrained case and bounded least squares the real one; after a failure the attainable set, a polytope, shrinks, and some demands are no longer possible.

A worked example

Four thrusters on a square of arm 0.5 m, limited to 0 to 600 N, with the effectiveness matrix mapping thrusts to $(F, M_x, M_y)$.

1. All four working. For $F = 1000$, $M_x = 60$, $M_y = -40$ the pseudoinverse gives (260, 300, 240, 200) N, inside the limits, and bounded least squares agrees to the digit: when the demand is attainable and the minimum-norm point is feasible, they coincide.
2. Thruster 1 lost. The three remaining thrusters have a unique exact solution (560, −20, 460) N: thruster 3 would have to push −20 N, which is infeasible.
3. Bounded least squares. The best feasible point is (0, 550, 0, 450) N, with thrust 1000 N and moments (50, −50): 10 N m off in each axis.
4. Authority. At 1000 N the largest moment along each axis, with the other held at zero, is 500 N m in both directions for all four. With thruster 1 lost it is 0 N m for positive roll and positive pitch and 500 N m for negative roll and negative pitch: half the directions are gone.
5. Infeasible thrust. A demand of F = 1400 N with thruster 1 lost cannot be met (the best balanced thrust is 1200 N). With equal weights the allocator returns F = 1327 N and moments of (−63, −63): the roll has the wrong sign. With the moments weighted 10 times the thrust it returns F = 1106 N and moments of (47, −47): thrust is sacrificed, attitude is kept.

import gnckit as g

# four thrusters on the corners of a square (arm 0.5 m); outputs: total thrust F (N), roll moment Mx and pitch moment My (N m)
B = [[1.0, 1.0, 1.0, 1.0],
     [0.5, 0.5, -0.5, -0.5],
     [0.5, -0.5, -0.5, 0.5]]
lo, hi = [0.0] * 4, [600.0] * 4
lo3, hi3 = [0.0] * 4, [0.0, 600.0, 600.0, 600.0]            # thruster 1 lost

def achieved(u):
    return [sum(B[i][j] * u[j] for j in range(4)) for i in range(3)]

def show(name, u):
    print(f'  {name:34s} u = [' + ', '.join(f'{x:6.1f}' for x in u) + ']  F, Mx, My = ' + ', '.join(f'{x:7.1f}' for x in achieved(u)))

v = [1000.0, 60.0, -40.0]
print('command F = 1000 N, Mx = 60 N m, My = -40 N m; four thrusters limited to 0 .. 600 N')
u = g.alloc_pinv(B, v)
show('pseudoinverse (minimum norm)', u)
print('  within limits:', all(lo[j] <= u[j] <= hi[j] for j in range(4)))
show('bounded least squares', g.allocate(B, v, lo, hi))
print('thruster 1 lost:')
u3 = g.alloc_pinv([[r[j] for j in (1, 2, 3)] for r in B], v)
print('  the three remaining columns give the unique solution', [round(x, 1) for x in u3], '(thruster 3 would need a negative thrust)' if min(u3) < 0 else '')
show('bounded least squares', g.allocate(B, v, lo3, hi3))

def error(vv, lo_, hi_):
    uu = g.allocate(B, vv, lo_, hi_)
    return max(abs(a - c) for a, c in zip(achieved(uu), vv))

def reach(lo_, hi_, F, d):
    """Largest moment along the direction d = (dMx, dMy), in N m, that can be produced exactly with the thrust held at F."""
    a, c = 0.0, 800.0
    for _ in range(40):
        mid = (a + c) / 2
        if error([F, mid * d[0], mid * d[1]], lo_, hi_) < 1e-3:
            a = mid
        else:
            c = mid
    return a
print('authority at F = 1000 N: the largest moment (N m) along each axis, with the other moment zero')
print('                      +Mx     -Mx     +My     -My')
for name, l_, h_ in (('all four working', lo, hi), ('thruster 1 lost', lo3, hi3)):
    r = [reach(l_, h_, 1000.0, d) for d in ((1, 0), (-1, 0), (0, 1), (0, -1))]
    print(f'  {name:18s} ' + ' '.join(f'{x:7.1f}' for x in r))
print('hover demand F = 1400 N with thruster 1 lost cannot be met (the best symmetric thrust is 1200 N); who pays?')
v = [1400.0, 60.0, -40.0]
show('equal weights', g.allocate(B, v, lo3, hi3))
show('moments weighted 10 times thrust', g.allocate(B, v, lo3, hi3, wv=[1.0, 10.0, 10.0]))

command F = 1000 N, Mx = 60 N m, My = -40 N m; four thrusters limited to 0 .. 600 N
  pseudoinverse (minimum norm)       u = [ 260.0,  300.0,  240.0,  200.0]  F, Mx, My =  1000.0,    60.0,   -40.0
  within limits: True
  bounded least squares              u = [ 260.0,  300.0,  240.0,  200.0]  F, Mx, My =  1000.0,    60.0,   -40.0
thruster 1 lost:
  the three remaining columns give the unique solution [560.0, -20.0, 460.0] (thruster 3 would need a negative thrust)
  bounded least squares              u = [   0.0,  550.0,    0.0,  450.0]  F, Mx, My =  1000.0,    50.0,   -50.0
authority at F = 1000 N: the largest moment (N m) along each axis, with the other moment zero
                      +Mx     -Mx     +My     -My
  all four working     500.0   500.0   500.0   500.0
  thruster 1 lost        0.0   500.0     0.0   500.0
hover demand F = 1400 N with thruster 1 lost cannot be met (the best symmetric thrust is 1200 N); who pays?
  equal weights                      u = [   0.0,  600.0,  126.7,  600.0]  F, Mx, My =  1326.7,   -63.3,   -63.3
  moments weighted 10 times thrust   u = [   0.0,  600.0,    0.0,  505.9]  F, Mx, My =  1105.9,    47.1,   -47.1

In robotics

Every vehicle with redundant effectors has an allocator, and its design is the design of the failure behaviour.

• Multirotors. Hexacopters and octocopters have more rotors than demands, and the allocator redistributes thrust when a rotor fails. The attainable set in yaw is the first thing lost, since yaw comes from small differences in torque.
• Aircraft. A modern aircraft has many control surfaces for three moments: elevons, flaps, canards, thrust vectoring. Allocation chooses which to use for efficiency, drag or signature, and reconfigures after a jam.
• Spacecraft. Reaction wheels, control-moment gyroscopes and thrusters are allocated with limits on wheel speed (momentum) and torque. Using the null space to unload momentum is the standard use of the freedom.
• Priorities. When the demand is infeasible the weights decide. A flight controller usually protects attitude over thrust, as the last example does, because a vehicle that loses attitude loses everything.
• Rate limits and dynamics. The simple box of this lesson ignores actuator rates. Rate limits are included by constraining the change per step, and dynamic allocation accounts for the lag.

The lab M5-L2: Fly with One Effector Lost flies this loss with Hopper's four thrusters. The upstream demand comes from the loops of M5-T3: Dynamic Inversion and Its Incremental Form, and a limit hit here must be reported back to them as saturation (the windup of M3-T7: Digital Control: Sampling, Delay and Anti-Windup).

Common misconceptions

Key takeaways

1. Allocation solves $\mathbf{B}\mathbf{u} = \mathbf{v}$ for more effectors than demands, with $\mathbf{u}_{\min} \le \mathbf{u} \le \mathbf{u}_{\max}$; the null space of B is the redundancy.
2. The weighted pseudoinverse gives the minimum-effort exact solution, ignoring limits.
3. Bounded least squares gives the closest feasible demand, and the weights set which axis is sacrificed first.
4. The attainable set is a polytope; after a failure it shrinks and may lose directions entirely.
5. For Hopper's thrusters, losing one at 1000 N removes positive roll and positive pitch moments with the other held at zero.
6. Priorities matter: protecting attitude over thrust keeps the vehicle controllable when the demand is infeasible.

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.

• T. A. Johansen and T. I. Fossen, "Control allocation: a survey", Automatica 49(5), 2013, a survey of the methods (pseudoinverse, constrained optimization, daisy chaining). Paid (Elsevier); the authors' pre-print is on their institutional page.
• R. Tedrake, Underactuated Robotics , for the optimization view of allocation in the chapters on trajectory and constrained control. Free to read; licence CC BY-NC-ND 4.0.
• W. C. Durham, "Constrained control allocation", Journal of Guidance, Control, and Dynamics 16(4), 1993, on the attainable moment set. Paid (AIAA).
• S. Boyd and L. Vandenberghe, Convex Optimization, Cambridge University Press, 2004, the sections on least squares with bounds. Free to read as the authors' PDF; the book is copyrighted.

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
  M5-T5: Adaptive Control and Its Risks

  Derive a model-reference adaptive law from a Lyapunov function, and explain parameter drift, loss of margin and why adaptive loops are hard to certify.

• Lab
  M5-L2: Fly with One Effector Lost

  A vehicle with four redundant thrusters loses one. Allocate the commanded force and moment over the remaining three within their limits, and report what authority is lost.

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

• Lab: Build a CBF Safety Filter
• Navigation M7-T2: Safety Filters: Control Barrier Functions
• UAV M5-T5: Attitude control
• UAV M5-T16: Disturbance rejection and wind compensation
• UAV M1-T6: Roll, pitch, yaw and thrust generation
• Linear Algebra M6-T7: The Moore-Penrose Pseudoinverse
• Linear Algebra M8-T9: Null-Space Control
• Robot Learning M9-T4: Hybrid Learning and Control Systems

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

• Control barrier functions and safety filters
• Geometric attitude control of a multirotor on SO(3)
• Disturbance observers for wind
• Learned controllers and residual policies