Simulating a 3D quadcopter from scratch | Home

This post is the second post in our series on quadcopter simulation.<br>If you want the simpler planar version first, the previous post derives and simulates the same ideas in 2D, but this post is fully standalone.<br>Today, we simulate a three-dimensional quadcopter.<br>We will derive the equations of motion, transform them into state-space form, and finally simulate the system in Python.<br>Along the way, we introduce quaternions and quaternion derivatives.

Problem setup and coordinates

The free-body diagram for the quadcopter is shown below.<br>We use a right-hand coordinate frame with the \(z\) axis for altitude.<br>The axes \(\{x, y, z\}\) are constant over time and form the world frame.<br>This frame of reference is inertial (does not accelerate).<br>The axes \(\{x', y', z'\}\) instead are always attached to the quadcopter center of mass and form the body frame, which changes over time.<br>The body frame is not an inertial frame of reference, as the drone accelerates.<br>Some equations of motion are easier to write in the world frame while others in the body frame.<br>Our model uses both.

The quadcopter is made of four rods of length \(\ell\) laid in a “+” pattern around the center \(C\).<br>There is a spinning propeller at the end of each rod, which generates thrust \(F_i\).<br>Propeller drag generates a torque \(\tau_{i} = \frac{F_i}{k}\) opposite to the propeller spin direction.<br>The quadcopter can have arbitrary position and attitude (i.e. its orientation).<br>Let \(\mathbf{p}\) be the position vector from origin \(O\) to center of mass \(C\).<br>Let \(q\) be the quaternion that rotates a vector’s coordinates from the body frame to the world frame, representing the attitude.

Quaternions

We briefly introduce quaternions here because they are the most convenient way to represent attitude in this model.<br>Quaternions, unit quaternions to be precise, are a way to represent rotations.<br>Rotations, especially in 3D, are strange objects as they don’t live in the usual \(\mathbb{R}^3\) like position or velocity vectors.<br>Instead, rotations live in a space called the 3D special orthogonal group, or \(\text{SO}(3)\).<br>Representing \(\text{SO}(3)\) objects in \(\mathbb{R}^3\) is possible, for example using Euler angles, but leads to issues like singularities and numerical instability.<br>For our purposes, representing attitude with quaternions leads to simpler, more efficient, and more numerically stable code.<br>For a geometric introduction to quaternions, I recommend this video by Freya Holmer.<br>We’ll talk more about quaternions, specifically about their derivatives, later in the post.

Deriving the equations of motion

To simulate the quadcopter, we need a differential equation that tells us how its state changes under a given motor input.<br>That starts with the rigid-body equations of motion: once we know the translational and rotational accelerations, we can package them into a first-order state-space model and integrate it numerically.

Having introduced our free body diagram, let’s write the Newton-Euler rigid body equations of motion.<br>In 2D we could work with three scalar equations, two for position (\(y, z\) axes) and one for rotation.<br>In 3D we have six equations: three for translation (in world frame), three for rotation (in body frame), written in vector form.<br>\(\mathbf{\dot{v}}\) is the time derivative of velocity, itself the derivative of position \(\mathbf{p}\).<br>\(\dot{\omega}\) is the time derivative of angular velocity, which is not the derivative of attitude (the two are connected via the kinematic matrix).<br>Finally, \(m\) is the mass and \(I\) is the moment of inertia.

\[\begin{aligned}

m \mathbf{\dot{v}} &= \sum_i \mathbf{F_i} && \text{(world frame)} \\<br>I \dot{\omega} + \omega \times I \omega &= \sum_i \mathbf{\tau_i} && \text{(body frame)}

\end{aligned}\]

Let’s now plug in the forces from the free-body diagram into the translation equations.<br>In world frame where the axes are \(\{x, y, z\}\), our quadcopter is subject to the gravitational force \(-m\mathbf{g}\) where \(\mathbf{g} = \left[0, 0, g\right]^T_{\text{world}}\).<br>Additionally, each motor produces thrust \(\mathbf{F_i}\) of magnitude \(F_i\), but this vector is only easy to write in body frame, where the motors always point straight up: \(\mathbf{F_i} = \left[0, 0, F_i\right]^T_{\text{body}}\).<br>To convert our simple expression for thrusts from body frame into world frame, we must rotate them.<br>To rotate the body frame vector to world frame we use the attitude quaternion \(q\).

We derive the rotation equations in the body frame by seeing how each force or torque affects each rotation axis.<br>We denote rotations about the body-frame axes by roll \(\phi\), pitch \(\theta\), and yaw \(\psi\).<br>If the second motor speeds up, increasing \(F_2\), roll \(\phi\) will increase.<br>If the fourth motor speeds up, increasing \(F_4\), roll \(\phi\) will decrease.<br>Thus the force couple \((F_2 - F_4)\), multiplied by arm length \(\ell\), will form the...