devol.dev

Quadcopter kinematics

A quadcopter has four propellers bolted to a rigid frame, all facing the same way. None of them tilts, none of them reverses, and the only thing any of them can do is push harder or softer along one fixed direction.

Out of that come six degrees of freedom, or four of them, depending on how you count. The gap between those two numbers is the entire subject.

650 g airframe, 23 cm across, 3.8:1 thrust to weight

Fly it

Click the view, then fly. W and S pitch, A and D roll, Q and E yaw, and the slider is the throttle. The four readouts are what the mixer is asking each motor for, as a fraction of what that motor has. Drag to orbit.

Click to flyW S pitch · A D roll · Q E yaw · drag to orbit
0.00
0.00
0.00
0.00
Click the view to take the controls.

Hold W and watch the machine tip forward before it goes anywhere. It has no way to push itself forward. It can only lean, so that some of the thrust that was holding it up starts pointing where it wants to go, and then it falls in that direction under control.

Four motors, four numbers

Start with one rotor. It sits at body-frame offset (x,y)(x, y) from the centre of mass and pushes along body zz with thrust ff. That push produces a torque about the centre of mass of r×Fr \times F, which for a force straight up is

τ=(yf, xf, 0)\tau = (y f,\ -x f,\ 0)

A rotor also drags the airframe around. The blade pushes air, the air pushes back, and the reaction appears as a torque about the rotor’s own axis opposing its rotation. That torque is proportional to the same thrust:

τz=σcf\tau_z = -\sigma\, c\, f

where σ\sigma is +1+1 for a counter-clockwise rotor and cc is the ratio of the drag and thrust coefficients. It has units of length, and on a five inch propeller it is about 16 mm.

Stack four rotors and every one of those contributions is linear in ff, so the whole of control allocation is one matrix:

[Tτxτyτz]=[1111y1y2y3y4x1x2x3x4σ1cσ2cσ3cσ4c][f1f2f3f4]\begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \begin{bmatrix} 1 & 1 & 1 & 1 \\ y_1 & y_2 & y_3 & y_4 \\ -x_1 & -x_2 & -x_3 & -x_4 \\ -\sigma_1 c & -\sigma_2 c & -\sigma_3 c & -\sigma_4 c \end{bmatrix} \begin{bmatrix} f_1 \\ f_2 \\ f_3 \\ f_4 \end{bmatrix}

This is the mixer. Four thrusts in, one force and three torques out. Every flight controller ever written has these four rows in it somewhere, usually as a table of plus and minus ones with the constants folded into the gains.

Four equations, four unknowns. Unlike the mecanum chassis, which has four wheels for three degrees of freedom, nothing here is redundant. Invert the matrix and each wanted wrench has exactly one set of rotor thrusts that produces it.

Where you put the rotors

The matrix does not care where the arms point, which makes the two common airframes a fair comparison.

X against plus

Two places to bolt four motors

Same motors, same arms, same mass. Switch the airframe and roll a little, then pitch a little, and watch how many of the four readouts move.

Click to flyW S pitch · A D roll · Q E yaw · drag to orbit
0.00
0.00
0.00
0.00
Click the view to take the controls.

On a plus frame the rotors sit on the axes. The left and right rotors have x=0x = 0, so they contribute nothing to pitch; the front and back rotors have y=0y = 0 and contribute nothing to roll. Each axis is driven by one pair. The mixer rows are as decoupled as they get, and the arithmetic is trivial.

On an X frame every rotor sits at 45 degrees, so every one of them has a moment arm of L/2L/\sqrt{2} about both tilt axes. All four motors work on every roll and every pitch.

That costs each rotor 29% of its leverage and pays it back twice over. With motors that can each give fmaxf_{max}, the most roll torque available is

plus: LfmaxX: 2Lfmax\text{plus: } L f_{max} \qquad\qquad \text{X: } \sqrt{2}\, L f_{max}

because the plus can only push one rotor up while the other comes down, where the X has two doing each. A 41% gain in authority on both axes, from rotating the frame 45 degrees and changing nothing else. That, and keeping the propellers out of a forward-facing camera, is why almost everything flying today is an X.

Yaw comes from drag, not from thrust

Roll and pitch come from thrust differences across an arm 8 cm long. Yaw comes from the drag reaction, through a constant of 16 mm.

That ratio, five to one on this airframe, is not a detail. Ask the tool for a pure yaw and watch the readouts spread five times wider than they do for the same torque in roll. Yaw is the axis that saturates first, responds slowest, and gets given up first when something has to give. On any multirotor you have ever flown, it was the weak one.

It is also the axis whose sign depends on assembly. The drag torques only cancel in a hover if two rotors turn one way and two the other, and only if the two that match are the ones diagonally opposite. Walk around the frame and the directions should alternate.

Three ways to fit four propellers

Prop direction

Pale rotor discs turn counter-clockwise, dark ones clockwise, and on a correct build the two diagonals match. This one starts with every propeller turning the same way. Fly it, then fix it.

Click to flyW S pitch · A D roll · Q E yaw · drag to orbit
0.00
0.00
0.00
0.00
Click the view to take the controls.

All four the same way. The drag torques add instead of cancelling. In the matrix, the last row becomes c-c times the first row, so the rank drops to three and yaw is no longer independent of thrust. Every wrench the rotors can produce obeys

τz=cT\tau_z = -c\,T

which says the machine cannot hover without spinning, and cannot stop spinning without falling. Hovering needs T=mgT = mg, and T=mgT = mg comes with 0.1 N m of yaw torque nobody asked for. The tool’s attitude loop commands zero yaw rate the whole time and gets ignored.

Notice what still works. It lifts, it rolls, it pitches, and it holds a lean. Everything you would check on a bench is fine.

Neighbours matched instead of opposites. Rank three again, and this time yaw is welded to roll:

τz=c2Lτx\tau_z = \frac{c\sqrt{2}}{L}\,\tau_x

Ask for roll, get yaw. Ask for yaw, get roll. No mixer can separate them again, because the separation was thrown away in the airframe, not in the software. This build flies, badly, in a way that reads as a tuning problem.

The rule that falls out: opposite rotors match, neighbours differ. It is one sentence and it is worth checking twice, because both failures above look like a control problem and neither is one.

It has to lean to go anywhere

The rotors push along body zz and nothing else. In the world frame,

p¨=TmR(ϕ,θ,ψ)e3ge3\ddot{p} = \frac{T}{m} R(\phi, \theta, \psi)\, e_3 - g\, e_3

Three equations of translation, and the only quantities on the right are one scalar TT and a direction the airframe is pointing. There is no term you can add to make it accelerate sideways while staying level. A quadcopter is underactuated: six degrees of freedom, four independent inputs.

Read the equation backwards and it becomes a recipe. Pick the acceleration you want, add gravity, and that vector is where the rotors have to point:

Tb3=m(p¨des+ge3)T\, b_3 = m(\ddot{p}_{des} + g\,e_3)

The length of the right-hand side is the thrust, the direction is the attitude. Two of the three attitude angles are decided by the acceleration you asked for, and yaw is left over, free to be anything. That is the one spare degree of freedom four rotors buy, and it is why a camera drone can fly a straight line while turning to look at something else.

The cosine you keep paying

Only the vertical component of the thrust holds the aircraft up. Lean by θ\theta and that component is TcosθT\cos\theta, so a lean costs lift at constant throttle:

z¨=Tmcosθg\ddot{z} = \frac{T}{m}\cos\theta - g

At 30 degrees, cosθ=0.87\cos\theta = 0.87. Thirteen percent of the lift is gone and the machine sinks while it accelerates.

Thrust, weight, and what is left

Why it sinks when you lean

Navy is thrust over mass, grey is gravity, rust is the sum: the acceleration. Hold W and watch the rust arrow tip below horizontal. Then tick tilt compensation and hold W again.

Click to flyW S pitch · A D roll · Q E yaw · drag to orbit
0.00
0.00
0.00
0.00
Click the view to take the controls.

The fix is one division. Ask for T/cosθT / \cos\theta instead of TT and the vertical component comes back to what it was:

x¨=gtanθz¨=0\ddot{x} = g\tan\theta \qquad\qquad \ddot{z} = 0

Every altitude-hold mode does this, and it is the difference between a machine that dives when you push the stick and one that does not. It is also where the tilt limit comes from: 1/cosθ1/\cos\theta runs away, and at 60 degrees the throttle demand has doubled.

When the mixer asks for more than the motors have

The inverse of that matrix is happy to return a negative thrust, or one above what the motor can produce. A propeller can do neither.

The naive fix, scaling all four thrusts by the same factor until they fit, is a good way to lose an aircraft. It scales the torques by that factor too, so at exactly the moment the machine is asking hardest for a correction, it gets a fraction of one.

Split the request instead. Let fˉ\bar{f} be the mean of the four thrusts and di=fifˉd_i = f_i - \bar{f} the deviations. The mean carries all of the total thrust and none of the torque; the deviations carry all three torques and no thrust. They can be traded separately, so:

  • if the deviations did_i span less than the motor’s range, slide fˉ\bar{f} up or down until everything fits. All three torques survive exactly. What gets lost is total thrust.
  • only when the spread alone is wider than the range does the attitude command get scaled.

Attitude first, altitude second. Losing height for half a second is recoverable and losing attitude is not. Hold full throttle and a hard roll in any of the tools above: the status line says which of those two trades it just made.

The code

Written the way a flight controller writes it, for an X frame with xx forward, yy left and zz up:

// The inverse of the mixer matrix, written out. Each line is one motor's
// share of the four commands, in the rotor order FL, FR, BL, BR.
//
//   a = L / sqrt(2), the moment arm of one rotor about either tilt axis
//   c = drag torque per newton of thrust, about 0.016 m
//
// Yaw is divided by c and the tilt axes by a, which is the five to one
// above: the same torque asked for in yaw spreads the four numbers five
// times further.
std::array<double, 4> allocate(double T, double roll, double pitch, double yaw,
                               double a, double c) {
    return {
        T / 4 + roll / (4 * a) - pitch / (4 * a) - yaw / (4 * c),   // FL
        T / 4 - roll / (4 * a) - pitch / (4 * a) + yaw / (4 * c),   // FR
        T / 4 + roll / (4 * a) + pitch / (4 * a) + yaw / (4 * c),   // BL
        T / 4 - roll / (4 * a) + pitch / (4 * a) - yaw / (4 * c),   // BR
    };
}

// What the fit had to give up. Attitude is the bad one: the torques
// delivered are smaller than the ones asked for, and `scale` says by how
// much. Collective only costs height.
enum class Mode { Ok, Collective, Attitude };

struct Mix {
    std::array<double, 4> thrusts;
    Mode mode;
    double scale;      // what survived of the attitude command
    double headroom;   // thrust still free above the spread
};

// Fit four thrusts into what four motors can do. The mean carries all of
// the total thrust and none of the torque, the deviations all three
// torques and no thrust, so the two can be traded separately.
Mix saturate(std::array<double, 4> thrusts, double maxThrust) {
    const double mean = (thrusts[0] + thrusts[1] + thrusts[2] + thrusts[3]) / 4;

    std::array<double, 4> dev = thrusts;
    for (double& d : dev) d -= mean;

    const double lo = *std::min_element(dev.begin(), dev.end());
    const double hi = *std::max_element(dev.begin(), dev.end());
    const double spread = hi - lo;

    // The deviations alone are wider than the motors can express, so
    // nothing preserves the attitude command now. Scale it down and rest
    // what is left on the bottom stop.
    if (spread > maxThrust) {
        const double scale = maxThrust / spread;
        for (double& d : dev) d *= scale;
        const double shift = -*std::min_element(dev.begin(), dev.end());
        for (int i = 0; i < 4; i++) thrusts[i] = dev[i] + shift;
        return {thrusts, Mode::Attitude, scale, 0.0};
    }

    // The spread fits. Slide the collective until it fits between the
    // stops too, which costs total thrust and keeps every torque exactly.
    const double shifted = std::min(std::max(mean, -lo), maxThrust - hi);
    for (int i = 0; i < 4; i++) thrusts[i] = dev[i] + shifted;

    // Whether the clamp bit. Untouched it hands mean straight back, so
    // this is a comparison against zero, with a tolerance for a mean
    // sitting a hair outside a stop.
    const Mode mode =
        std::fabs(shifted - mean) > 1e-12 ? Mode::Collective : Mode::Ok;

    return {thrusts, mode, 1.0, maxThrust - spread};
}

Most stacks write the four rows above as a table of ±1\pm 1 and fold aa and cc into the gains, which is the same thing with the units taken out. Doing that is fine, right up until someone changes the arm length and wonders why the roll gains no longer suit.

The signs depend entirely on the frame. This page uses xx forward, yy left, zz up, which puts positive roll to the right and positive pitch nose down. Aerospace convention puts zz down and flips both. Copying a mixer out of one codebase into another without checking which convention it was written in is how a machine ends up rolling the wrong way on its first flight.

What the tool is not doing

The rotors here go from one thrust to another instantly. Real motors and propellers take 30 to 80 ms to change speed, which is a delay inside the fastest loop on the aircraft, and it sets the ceiling on how hard the rate gains can be pushed before it oscillates.

Thrust is treated as proportional to the command. It is closer to proportional to the square of the rotor speed, and it falls off as the machine flies faster and as the battery sags.

Nor is there any aerodynamics past a linear drag term. No ground effect, no blade flapping, no wind, no vortex ring state waiting for a fast descent into your own downwash.

What is here is the part that is exact: the geometry of the four arms, the matrix it produces, and the fact that only one direction of the aircraft can ever push.