Today I'll go through some of the key steps I took in order to implement a simple 3D simulator of a quadcopter. Some Rust code snippets will be shown for reference. For a full implementation please check out my Github project.

0. Notations

• $v$ is a scalar.
• $\mathbf{v}$ is a vector.
• $\mathbf{R}$ is a matrix.

1. Definitions

First, let me define some vocabulary which I will use extensively later in this post.

1.1. Body frame

The body frame is the frame of the quadcopter.

The three vectors $\color{red}{\mathbf{x_b}}, \color{lime}\mathbf{y_b}$ and $\color{royalblue}\mathbf{z_b}$ define the basis centered on the quadcopter and determines its orientation as well. I chose the vector $\color{royalblue}\mathbf{z_b}$ to represent the direction that is normal to the quad, facing upward. The quad hereby lives in the plane $\color{red}\mathbf{x_b}\color{lime}\mathbf{y_b}$, with each arm aligned with $\color{red}\mathbf{x_b}$ and $\color{lime}\mathbf{y_b}$.

Figure $2$: The body frame.

1.2 Roll, pitch and yaw.

The orientation of the quadcopter can be fully described by three angles: the roll, pitch and yaw (notated $\theta$, $\phi$ and $\psi$). Each one represents ab angle of rotation around the body frame vectors $\color{red}\mathbf{x_b}, \color{lime}\mathbf{y_b}$ and $\color{royalblue}\mathbf{z_b}$.

Figure $3$: Roll, pitch and yaw rotations.

One vector of interest is also the derivatives of those quantities wrt. time: I notate the vector of angular velocities (not the angular velocity vector) as $\dot{\mathbf{\Theta}} = [\dot{\theta}, \dot{\phi}, \dot{\psi}]^T$.

1.3 Inertial frame

The inertial frame is the frame of reference on which no acceleration is applied and stays fixed. For us humans, the earth looks like an inertial frame locally. We describe several key states in the inertial frame, like the acceleration $\mathbf{a}$ or the velocity $\mathbf{v}$.

For it, we define a orthonormal basis composed of classical cartesian vectors: $\mathbf{x_i}, \mathbf{y_i}$ and $\mathbf{z_i}$. I chose $\mathbf{z_i}$ to represent altitude, while the ground belongs to the plane $\mathbf{x_i}\mathbf{y_i}$.

Figure $4$: From body to inertial frame.

In order to switch between reference frames, we can use the following matrix of rotation:

$$\mathbf{R}= \begin{bmatrix} 1 & 0 & 0 \cr \cos \theta & \sin \theta &0 \cr 0 &-\sin \theta & \cos \theta \cr \end{bmatrix} \begin{bmatrix} \cos \phi & 0 & -\sin \phi \cr 0 & 1 & 0 \cr \sin \phi & 0 & \cos \phi \cr \end{bmatrix} \begin{bmatrix} \cos \psi & \sin \psi & 0 \cr -\sin \psi & \cos \psi & 0 \cr 0 & 0 & 1 \cr \end{bmatrix} $$

This matrix $\mathbf{R}$ can be used in a forward way to convert vectors from the body frame to the inertial frame:

$$\mathbf{a}_i = \mathbf{R}\mathbf{a}_b$$

See below on implementation in Rust, using nalgebra:

// Rotation matrix from body frame to intertial frame.
pub fn get_rotation_matrix(angles: Vector3<Real>) -> Matrix3<Real> {
    let roll = angles[0]; // roll
    let pitch = angles[1]; // pitch
    let yaw = angles[2]; // yaw
    let r1 = matrix![1.0, 0.0, 0.0; 0.0, roll.cos(), roll.sin(); 0.0, -roll.sin(), roll.cos()];
    let r2 = matrix![pitch.cos(), 0.0, -pitch.sin(); 0.0, 1.0, 0.0; pitch.sin(), 0.0, pitch.cos()];
    let r3 = matrix![yaw.cos(), yaw.sin(), 0.0; -yaw.sin(), yaw.cos(), 0.0; 0.0, 0.0, 1.0];
    let r = r3.transpose() * r2.transpose() * r1.transpose();
    r
}

2. Kinematics

2.1 Acceleration

The thrust delivered by the motors is directly proportional to the sum of each rotor's angular velocity.

If we notate $\omega_i$ the angular velocity of the i-th rotor, let also notate its square $\gamma_i = \omega_i^2$ for clarity. The thrust is equal to the sum of all rotor contributions: $$T = k\cdot\sum_i \gamma_i$$ The actual vector acceleration has a magnitude of the thrust and the direction of the quad's normal. That is to say that the acceleration $a_b$ is equal to : $$\mathbf{a} = T \cdot \mathbf{z_b}$$ The resulting force applied on the quad of mass $m$ should then be (according the Newton's second law): $$F_{Tb} = m\cdot \mathbf{a}$$ We use the matrix $\mathbf{R}$ in order to compute the force vector in the inertial frame: $$F_{Ti} = \mathbf{R}^T F_{Tb}$$ The acceleration updates the velocity vector $\mathbf{v}$.

In my simulator, I used the rapier3D physics engine to handle the physics steps of velocity/positions update and manage the properties of my quad and the floor. The main reason why I chose to rely on an external engine was the handling of rigid body collisions which was out of scope for my project.

Again, implementation code:

// Compute acceleration in normal quad direction.
pub fn compute_acceleration(
    inputs: Vector4<Real>,
    angles: Vector3<Real>,
    m: Real,
    k: Real,
) -> Vector3<Real> {
    let roll_pitch_yaw = vector![angles.x, angles.y, angles.z];
    let normal = get_rotation_matrix(roll_pitch_yaw) * Vector3::new(0.0, 0.0, 1.0);
    let t = normal * thrust(inputs, k);
    let a = 1.0 / m * t;
    a
}

Alone, this isn't enough to simulate the quad behavior, as we are always applying a force along its normal. We need to compute the torques produced by the rotating propellers and update the angular velocity of our quad.

2.2 Torque

Rotating propellers at each arm ends with different angular velocities produces torques which induce the quad's rotation.

We can compute the torque in function of $\gamma_i$ in the body frame: $$ \begin{aligned} \tau_{\theta} &= l \cdot k \cdot(\gamma_0 - \gamma_2) \cr \tau_{\phi} &= l\cdot k \cdot(\gamma_1 - \gamma_3) \cr \tau_{\psi} &= b\cdot(\gamma_0 - \gamma_1 + \gamma_2 - \gamma_3) \end{aligned}$$

• $k$ aerodynamic mother constant.
• $l$ arm's length.

Because of the "plus/+" configuration used, only two rotors are responsible for torque production along the $\theta$ and $\phi$ angles of rotation.

In order to apply those values to the torque expressed in the inertial frame, we have to convert it back using the inverse of rotation matrix $\mathbf{R}$:

$$\mathbf{\tau}_i =\mathbf{R}^{T}\tau_b$$

With $\tau_b=[\tau_{\theta}, \tau_{\phi}, \tau_{\psi}]^T$.
The torque updates the angular rotation vector (axis of rotation and its magnitude).

Implementation code:

// Torque computation from input squared velocities
pub fn compute_torque(inputs: Vector4<Real>, l: Real, b: Real, k: Real) -> Vector3<Real> {
    let tau = vector![
        l * k * (inputs[0] - inputs[2]),
        l * k * (inputs[1] - inputs[3]),
        b * (inputs[0] - inputs[1] + inputs[2] - inputs[3])
    ];
    tau
}

3. Control

The goal here is not simply to run the simulation and look at our quadcopter fall to the ground: controlling the quad is a large part of the fun.

Many options are available as control schemes for a quadcopter. I implemented what is commonly called the acro mode (for acrobatics), where control inputs are directly mapped to angular velocities.

So, there are actually several things left to keep in mind:

• Mapping axis positions to quad's angular velocities (or $\dot{\theta}$.
• Manage some level of thrust with throttle command.
• Derive rotors' angular velocities (or $\omega_i$) to achieve such control.

3.1 P Controller

We will implement this as a P controller (P for proportional).

First, we'll sense the actual angular velocities $\dot{\theta}$ (such measure would come from a gyroscope in reality) and compare them to the requested input angular velocities $\dot{\theta}_c$ comming from our radio controller.

$$\mathbf{e} = [e_{\theta}, e_{\phi}, e_{\psi}]^T = \mathbf{k_p}(\dot{\theta} - \dot{\theta}_c)=\begin{bmatrix} k_p^\theta(\dot{\theta}-\dot{\theta}_c) \cr k_p^\phi(\dot{\phi}-\dot{\phi}_c) \cr k_p^\psi(\dot{\psi}-\dot{\psi}_c) \end{bmatrix}$$

We also fisetx the total thrust $T$ to be equal to some affine relation with the throttle command $t_c$: $$T = a_t\cdot t_c + b_t$$

Then, we derive the squared angular velocities of the rotors $\gamma_i$, ensuring their sum is equal to $T$.

$$ \begin{aligned} \gamma_0 &= \frac{T}{4}-\frac{2b e_{\phi}I_{11}+e_{\psi}I_{33}kl}{4bkl} \cr \gamma_1 &= \frac{T}{4}+\frac{e_{\psi}I_{33}}{4b}-\frac{e_{\theta}I_{22}}{2kl} \cr \gamma_2 &= \frac{T}{4}-\frac{-2b e_{\phi}I_{11}+e_{\psi}I_{33}kl}{4bkl} \cr \gamma_3 &= \frac{T}{4}+\frac{e_{\psi}I_{33}}{4b}+\frac{e_{\theta}I_{22}}{2kl} \end{aligned} $$

• $I$ : angular mass.

Implementation:

// Computing error on each euler angle
let e_phi = kp * (phi_dot + compute_expof(command.pitch));
let e_theta = kp * (theta_dot + compute_expof(command.yaw));
let e_psi = kp * (psi_dot + compute_expof(command.roll));

// Simple moment of inertia
let i: Matrix3<Real> = Matrix3::identity() * 1.0;

// Setting minimum thrust to barely uplift
let mut thurst = 3.0 + command.throttle.sqrt() * 4.0;
thrust /= 4.0;

let b = constants.b;
let l = constants.l;
let k = constants.k;

// Deriving correction inputs to minimize error
// input_i is the rotor angular velocity squared
let gamma_0 = thrust - (2.0 * b * e_phi * i.m11 + e_psi * i.m33 * k * l) / (4.0 * b * k * l);
let gamma_1 = thrust + (e_psi * i.m33) / (4.0 * b) - (e_theta * i.m22) / (2.0 * k * l);
let gamma_2 = thrust - (-2.0 * b * e_phi * i.m11 + e_psi * i.m33 * k * l) / (4.0 * b * k * l);
let gamma_3 = thrust + (e_psi * i.m33) / (4.0 * b) + (e_theta * i.m22) / (2.0 * k * l);

3.2 Exponential rates

The actual angular velocity requested is not linear in function of axis movement. Actual rates follow exponential laws for increased sensitivity towards high input values (enables fast movements for rolls or flips).

I reimplemented the Betaflight formula (well known real flight controller firmware) for my simulator:

fn compute_expof(rc_command: f32) -> f32 {
    // https://github.com/ctzsnooze/betaflight/blob/b61d641bfc743e3cc58da05b50c869c0e7f59c7c/src/main/fc/rc.c
    let expo_factor = rc_command.abs() * (rc_command.powf(5.0) * 0.5 + rc_command * (1.0 - 0.5));
    let center_sensitivity: f32 = 0.5;
    let stick_movement: f32 = (0.6 * 10.0 - center_sensitivity).max(0.0);
    let angle_rate: f32 = rc_command * center_sensitivity + stick_movement * expo_factor;
    angle_rate
}

4. Conclusion

The implementation details that are involved to actually build a a working simulator in Rust with inputs control, 3D visualization as well as metrics logging involves a lot more work than was depicted here, but I hope I covered the fundamentals of quad dynamics to enable you build your own toy example !

If you were to be interested in testing the simulator I developed using the very same lines of this post, feel free to visit the Github page.

5. Reference

[1] Great reference on the actual derivation of the formulas used and the physical hypothesis behind them. Caution, I am not using the same notations / controller.