M1-T5: Six-degree-of-freedom UAV dynamics

Learning objectives

After this lesson, you should be able to:

• List the 12 states of a rigid-body UAV and their frames.
• Write the translational and rotational equations of motion with thrust and torque as inputs.
• Explain why a quadrotor is underactuated and how tilt produces horizontal acceleration.
• Integrate the model numerically and verify hover and a free fall.

Conceptual explanation

A rigid body in space has six degrees of freedom: three of position and three of orientation. With their rates, the state has twelve numbers: position $p$ (3), velocity $v$ (3), attitude (roll, pitch, yaw or a quaternion) and body angular rate $\omega (3)$.

A quadrotor has only four inputs: the four motor thrusts, equivalently a total thrust $T$ and three body torques $\tau _{x}$, $\tau _{y}$, $\tau _{z}$. Six degrees of freedom driven by four inputs makes it underactuated: it cannot accelerate sideways directly. To move horizontally it must first tilt, so that part of the thrust points sideways.

Engineering intuition

Think of the vehicle as a thrust vector attached to a rotating platform. Translation is fast to understand: weight pulls down, thrust pushes along the body $z$ axis. Rotation is the slow, delicate part: torques from differential thrust change $\omega$, $\omega$ changes orientation, orientation redirects thrust.

This chain, torque → angular rate → attitude → thrust direction → acceleration → velocity → position, is why controllers are cascaded. Each stage is an integrator or two away from the next. The inner (attitude) loop must be several times faster than the outer (position) loop, otherwise the outer loop sees a lagging plant and oscillates. You can see that in the Quadrotor Dynamics Simulator when the position gains are raised.

Mathematical formulation

Translation (world frame, z up)

Why is the weight term negative?

The weight is not negative: its size is the positive number $mg$ (14.7 $N$ for a 1.5 kg vehicle). The sign gives its direction. Here the world frame has $z$ pointing up and $\hat{\mathbf{z}} = [0,0,1]^{\top}$ is the unit vector along it. Gravity pulls down, opposite to ẑ, so the weight force vector is

On the vertical axis, level thrust gives +T and weight gives −mg, so $m\dot v_z = T - mg$: the vehicle climbs if $T$ > mg, hovers if $T$ = mg and sinks if $T$ < mg. In a z-down frame such as NED, $\hat{z}$ points down, so the same weight is written +mg $\hat{z}$ and thrust −$T$ $\hat{z}$; the physics is identical and only the sign convention changes. The code below uses $z$ up, which is why it subtracts $g$.

Rotation (body frame)

$p$, $v$

position and velocity in the world frame ($m$, m/s).

$R$

rotation matrix from body to world (lesson M1-T4).

$T$

total thrust, $T=f_{1}+f_{2}+f_{3}+f_{4}(N)$.

$J$

inertia matrix, usually diagonal $\operatorname{diag} (J_{x},\,J_{y},\,J_{z})$ for a symmetric airframe (kg·$m^{2}$).

$\omega$

angular velocity of the body in the body frame (rad/s).

$\tau$

torques about the body axes ($N$·$m$), produced by differential motor thrust (lesson M1-T6).

$\omega$ × J$\omega$

gyroscopic term: small for a symmetric vehicle at modest rates.

The Euler kinematics for the ZYX convention, with $\omega =(p,\,q,\,r)$, are:

The term $1/\cos \theta$ is the gimbal-lock singularity at $\theta =\pm 90^{\circ}$; avoid Euler angles for integration in real flight code.

Hover and small-angle behaviour

At hover, $\dot{v}=0$ and attitude is level, so $T=m\,g$. For small angles the horizontal accelerations are $a_{x}\approx g\,\theta$ and $a_{y}\approx -g\,\phi$ (with $x$ forward, $y$ left, $z$ up), which is the link the position controller uses to turn a desired acceleration into a desired tilt.

Architecture and data flow

In a flight stack the model appears three times, each time with different fidelity:

• Simulator: full nonlinear model with motor dynamics, drag and wind (this lesson).
• Controller design: a linearisation about hover (lesson M5-T7 uses it for LQR) or a simplified point-mass for MPC (lesson M5-T8).
• Estimator: the IMU is treated as measuring $\omega$ and specific force, and the model is used for prediction (module 3).

Real-world UAV applications

• Choosing motors and batteries: $T=m\,g$ and the margin for manoeuvres follow from this model.
• Simulation-first controller tuning before the first flight.
• Predicting how payload mass and centre-of-mass offsets change hover trim.
• Model-based estimation and model predictive control of aggressive flight.

Implementation example

This runs as shown: it integrates the translation and rotation equations from rest, checks that hover thrust $T=m\,g$ keeps the vehicle still, and that zero thrust gives a free fall of ½ $g\,t^{2}$.

import numpy as np
g = 9.81

def rot(phi, th, psi):
    cp, sp, ct, st, cy, sy = np.cos(phi), np.sin(phi), np.cos(th), np.sin(th), np.cos(psi), np.sin(psi)
    return np.array([[cy*ct, cy*st*sp - sy*cp, cy*st*cp + sy*sp],
                     [sy*ct, sy*st*sp + cy*cp, sy*st*cp - cy*sp],
                     [-st,   ct*sp,            ct*cp]])

def step(x, T, tau, m, J, dt):
    """x = [p(3), v(3), euler(3), omega(3)], z up. Semi-implicit Euler."""
    p, v, e, w = x[0:3], x[3:6], x[6:9], x[9:12]
    a = rot(*e) @ np.array([0, 0, T]) / m - np.array([0, 0, g])
    wdot = np.linalg.solve(J, tau - np.cross(w, J @ w))
    v = v + a * dt
    p = p + v * dt
    w = w + wdot * dt
    phi, th = e[0], e[1]
    W = np.array([[1, np.sin(phi) * np.tan(th), np.cos(phi) * np.tan(th)],
                  [0, np.cos(phi), -np.sin(phi)],
                  [0, np.sin(phi) / np.cos(th), np.cos(phi) / np.cos(th)]])
    e = e + (W @ w) * dt
    return np.concatenate([p, v, e, w])

m, J, dt = 1.5, np.diag([0.0096, 0.0096, 0.018]), 0.002
x = np.zeros(12)
for _ in range(1000):                       # hover for 2 s: T = m g, no torque
    x = step(x, m * g, np.zeros(3), m, J, dt)
assert np.allclose(x[0:3], 0, atol=1e-9)

x = np.zeros(12)
for _ in range(500):                        # free fall for 1 s
    x = step(x, 0.0, np.zeros(3), m, J, dt)
print("fell", round(-x[2], 3), "m; analytic 0.5*g*t^2 =", round(0.5 * g, 3))
assert abs(-x[2] - 0.5 * g) < 0.02

x = np.zeros(12); x[6] = 0.2                # 0.2 rad roll with hover thrust: it accelerates sideways
for _ in range(100):
    x = step(x, m * g, np.zeros(3), m, J, dt)
assert x[4] < 0                             # roll right-wing-down in z-up/y-left pushes toward -y
print("lateral velocity after 0.2 s:", round(x[4], 3), "m/s")

Common engineering mistakes

Failure modes

Debugging approach

1. Test the model alone first: hover thrust must keep it still and zero thrust must fall at $g$.
2. Apply a small constant roll torque and check the sign and magnitude of the resulting angular acceleration against $\tau /J_{x}$.
3. Compare simulated and logged step responses of attitude: the time constant and overshoot reveal missing lag.
4. Check energy: with no thrust and no drag, kinetic plus potential energy must be conserved to integration accuracy.

Practical exercises

1. Derive the thrust needed to hold altitude at $30^{\circ}$ roll and explain why a vehicle with $T/W=1.1$ cannot do it.
2. Linearise the model about hover and show that $\ddot{z}=(\delta T)/m$, $\ddot{\phi }=\tau _{x}/J_{x}$, and $\ddot{x}=g\,\theta$.
3. Add linear drag −$c$ $v$ to the code and find the terminal speed in free fall for $c=0.3\,\mathrm{kg}/\mathrm{s}$.
4. Replace Euler angles by a quaternion integration and compare after a $90^{\circ}$ pitch manoeuvre.

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.

• R. Mahony, V. Kumar, P. Corke, “Multirotor aerial vehicles: modelling, estimation and control of quadrotor”, IEEE RAM, 2012
• D. Mellinger, V. Kumar, “Minimum snap trajectory generation and control for quadrotors”, ICRA 2011 (model and differential flatness)
• PX4 documentation: multicopter control architecture

What’s next