Drone Physics

Coordinate Systems

A multi-rotor UAV is modeled with six degrees of freedom: the three linear axes for linear motion: $x, y, z$, and the three angular axes for rotational motion: $\phi, \theta, \psi$.

To use coordinates, a coordinate system needs to be agreed upon. Typically the North-East-Down (NED) system is used. The direction of the positive $x$ axis is considered "forward"/North orientation. The positive $y$ axis is "right"/East and the positive $z$ axis is "down". This is a right-handed coordinate system; positive rotation about an axis in the direction of the thumb is in the direction of the curled fingers. For example, a positive z-rotation goes from +x to +y.

Two reference frames are used for representing the state of the body:

• Nominal, inertial, reference frame $n$ is the static frame of reference where the axes are aligned with arbitrary, global directions. They are represented as column vectors $\hat{n} = [\hat{x}^n, \hat{y}^n, \hat{z}^n]^T$.
• Body-fixed, non-inertial, reference frame $b$ has the axes aligned with respect to the center of gravity of the rigid body in motion. They are represented as column vectors $\hat{b} = [\hat{x}^b, \hat{y}^b, \hat{z}^b]^T$.

The body frame moves and rotates with the vehicle. Consider the origin of the body frame attached to the center of mass of the drone.

Linear Representation

The body-fixed reference frame $b$ is affixed to the center of mass of the vehicle. That is, the displacement of the vehicle from the body frame is always 0. The position of the vehicle is represented by the displacement of the body frame from the inertial frame, $\hat{r}^n$. The velocity of the vehicle is the velocity of the body frame relative to the inertial frame. Here, we choose to represent velocity in coordinates of the body frame.

Angular Representation

Orientation of a body in the inertial reference frame follows the Tait-Bryan angles convention. That is, orientation can be described by three sequential rotations: yaw ($\psi$), pitch ($\theta$), and roll ($\phi$) - in that order. The order of rotations matters.

Starting from the inertial frame $\hat{n}$, yaw $\psi$ is rotation $R(\psi)$ of the body frame about the inertial $z$ axis. Starting from this yaw-ed frame $\hat{n}\psi$, pitch $\theta$ is rotation $R(\theta)$ about the new $y$ axis. And starting from this yaw-ed and pitch-ed frame $\hat{n}{\psi,\theta}$, roll $\phi$ is the final rotation $R(\phi)$ about the new $x$ axis. The final product, $\hat{n}_{\psi,\theta,\phi}$ is the body reference frame.

The angular velocity of the vehicle, $\hat{w}^T=[\omega_x,\omega_y,\omega_z]$, is the instantaneous rotational rate of the body frame about itself. Whereas each angle of orientation is defined in its own reference frame ($\hat{n}, \hat{n}{\psi}, \hat{n}{\psi,\theta}=\hat{b}$) during a sequence of ordered rotations from the inertial axes to the body frame (yaw, then pitch, then roll). Roll rotation happens last and the result is the body frame. Therefore, $\omega_x$, the rate about the x-axis equals the roll rate, $\dot{\phi}$. Pitch happens after the initial yaw, but before roll. Therefore, $\omega_y$, the rate about the y-axis, is the pitch rate in the frame $\hat{n}_{\psi}$ rotated by roll $\phi$ to get the body frame. Finally, yaw rotation happens first in the inertial frame. Therefore, $\omega_z$, the rate about the body frame's z-axis, is the yaw rate in the inertial frame rotated by pitch $\theta$ and roll $\phi$.

This can be expressed as a matrix equation:

$$
\begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} = R(\phi)\cdot R(\theta) \begin{bmatrix} 0 \ 0 \ \dot{\psi} \end{bmatrix} + R(\phi) \begin{bmatrix} 0 \ \dot{\theta} \ 0 \end{bmatrix} + \begin{bmatrix} \dot{\phi} \ 0 \ 0 \end{bmatrix}
$$

$$
\begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} = \begin{bmatrix} \dot{\phi} + \dot{\psi} \sin{\theta}\ - \dot{\psi} \sin{\phi} \cos{\theta} + \dot{\theta}\ \dot{\psi} \cos{\phi} \cos{\theta} \end{bmatrix}
$$

Reconciling Inertial and Body Frames

A body frame may be different from the inertial frame due to (1) displacement and (2) rotation. The body frame's origin is fixed to the origin of the UAV. Therefore, the displacement of the body frame from the inertial frame is the inertial position of the UAV: $\hat{r}^n=[x,y,z]^T$.

A vector relative to the origin of the body frame will appear rotated if displaced to the origin of the inertial frame. Given a vector in the inertial frame $\hat{\mathcal{V}}^n=[x^n,y^n,z^n]^T$ and the same vector at the origin of the body frame $\hat{\mathcal{V}}^b=[x^b,y^b,z^b]^T$, the rotation matrix from the body to inertial reference frames $R_b^n$ is defined as, where each rotation matrix is:

$$
\begin{align}
R(\phi) = \begin{bmatrix} 1 & 0 & 0 \ 0 & c\phi & -s\phi \ 0 & s\phi & c\phi \end{bmatrix}
\end{align}
$$

$$
\begin{align}
R(\theta) = \begin{bmatrix} c\theta & 0 & s\theta \ 0 & 1 & 0 \ -s\theta & 0 & c\theta \end{bmatrix}
\end{align}
$$

$$
\begin{align}
R(\psi) = \begin{bmatrix} c\psi & -s\psi & 0 \ s\psi & c\psi & 0 \ 0 & 0 & 1 \end{bmatrix}
\end{align}
$$

$$
\begin{align}
\hat{\mathcal{V}}^b &= R(\phi)\cdot R(\theta) \cdot R(\psi) \cdot \hat{\mathcal{V}}^n \
\hat{\mathcal{V}}^b &= R_n^b \hat{\mathcal{V}}^n \
\begin{bmatrix} x^b \ y^b \ z^b \end{bmatrix} &= \begin{bmatrix} c\psi c\theta & - s\psi c\theta & s\theta\ s\phi s\theta c\psi + s\psi c\phi & - s\phi s\psi s\theta + c\phi c\psi & - s\phi c\theta\ s\phi s\psi - s\theta c\phi c\psi & s\phi c\psi + s\psi s\theta c\phi & c\phi c\theta \end{bmatrix} \begin{bmatrix} x^n \ y^n \ z^n \end{bmatrix}
\end{align}
$$

Here $c | s$ of $\phi | \theta | \psi$ refer to the cosine and sine respectively. This rotation is useful when looking at forces acting in the body frame from an inertial viewpoint. The above transforms a vector instantaneously between rotated reference frames.

Reconciling Derivatives in Rotating Frames

If, however, the vector is changing while the reference frame is rotating, then the rate of change is not as simple. The rate of change of a vector is due to (1) the change of the vector quantity itself in the body frame, and (2) the change of the frame coordinates. That is, in a body frame rotating with instantaneous angular velocity $\hat{\omega}=[\omega_x,\omega_y,\omega_z]$ about its axes, the time-derivative of a vector in the body frame $\hat{\mathcal{V}}^b = \hat{b} \cdot \hat{\mathcal{V}}$ as measured in a co-located inertial frame is given by the Transport theorem:

$$
\begin{align}
\frac{d \hat{\mathcal{V}}^b}{d t} &= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \frac{d \hat{b}}{d t} \cdot \hat{\mathcal{V}} \
&= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \hat{\omega} \times \hat{\mathcal{V}}^b \
&= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \begin{bmatrix} 0 & -\omega_z & \omega_y \ \omega_z & 0 & -\omega_x \ -\omega_y & \omega_x & 0 \end{bmatrix} \hat{\mathcal{V}}
\end{align}
$$

Transport Theorem and the Cross Product

Assume the body frame has instantaneous angular velocity $\hat{\omega}$ about each axis. Each basis vector will see a rotation over a time interval $dt$. For small values of $dt$, the angular displacement is small, $\hat{\omega} dt$, and the arc drawn by the tip of the unit vector can be approximated as a straight line of magnitude $1 \cdot \hat{\omega} dt$ (arc length equals radius times angle in radians):

$$
\hat{x}^b \rightarrow \hat{x}^b + 0\hat{x}^b + \omega_z dt \hat{y}^b - \omega_y dt \hat{z}^b \
\hat{y}^b \rightarrow \hat{y}^b - \omega_z dt \hat{x}^b + 0 \hat{y}^b + \omega_x dt \hat{z}^b \
\hat{z}^b \rightarrow \hat{z}^b + \omega_y dt \hat{x}^b - \omega_x dt \hat{y}^b + 0 \hat{z}^b
$$

In matrix form, each column vector represents the change to the basis vector:

$$
\hat{b}{t+dt} = \hat{b}{t} + \begin{bmatrix} 0 & -\omega_z & \omega_y \ \omega_z & 0 & -\omega_x \ -\omega_y & \omega_x & 0 \end{bmatrix} dt
$$

The rate of change of the basis vectors of the reference frame is:

$$
\frac{d \hat{b}}{dt} = \begin{bmatrix} 0 & -\omega_z & \omega_y \ \omega_z & 0 & -\omega_x \ -\omega_y & \omega_x & 0 \end{bmatrix}
$$

The matrix multiplication is equivalent to a cross product with the vector $[\omega_x, \omega_y, \omega_z]^T$. The additive term is a fictitious force that accounts for non-inertial frame. Once that is accounted for, the dynamics can be solved for like an inertial frame. This can be used to integrate accelerations in the body frame to find body-frame velocity.

The State of the Vehicle

The vehicle dynamics are completely represented by 12 state variables: linear and angular displacement in each dimension, and their time derivatives:

• $\hat{r}^n=[x,y,z]^T$ are the navigation coordinates in the inertial frame. This is the linear displacement of the body frame from the inertial frame.
• $\hat{v^b}=[v^b_x, v^b_y, v^b_z]^T$ is the velocity of the vehicle along the body frame axes.
• $\hat{\Phi}=[\phi, \theta, \psi]^T$ is the orientation of the body reference frame $b$ in Euler angles (roll, pitch, yaw) with reference to the inertial reference frame.
• $\hat{\omega}=[\omega_x, \omega_y, \omega_z]^T$ is the instantaneous angular velocity along each of the body frame axes.

Dynamics of Multi-rotor UAVs

The previous section enumerated the variables that describe the vehicle. This section describes how variables change over time. Since state is split into linear and angular variables, the dynamics equations will be split into linear and rotational equations.

Here, we choose to represent forces and moments in the body frame coordinates:

• $\hat{F}^b=[F_x^b,F_y^b,F_z^b]^T$ are the net forces along the three body frame axes, where $\hat{F}^b=R_n^b \hat{F}^n$.
• $\hat{\tau}^b=[\tau_x^b,\tau_y^b,\tau_z^b]^T$ are the moments along the three body axes, where $\hat{\tau}^b=R_n^b \hat{\tau}^n$.

The forces acting on the vehicle are thrusts and gravity. Moments are generated when the forces are applied at a distance from the center of gravity. (We can incorporate drag, but we assume it's insubstantial for now.)

The force of gravity is $\hat{F_g}^n=[0,0,mg]$ in the inertial reference frame. In the body frame it becomes $\hat{F_g}^b=R^b_n \hat{F_g}^n$. The net force of the $p$ propellers in the body frame (thrust) is $\hat{T}=[0,0,\sum_i^p T_i]^T$, where $T_i$ is the thrust of the $i$th propeller. Thus, the total force acting on the center of mass is $\hat{F}^b=\hat{T} + \hat{F_g}^b$.

The multirotor is modeled as a rigid body. A rigid body has a constant mass distribution relative to its center of gravity.

Linear Equations of Motion

Fundamental to the dynamics is Newton's second law of motion: force is the rate of change of momentum. Here it is written in terms of body-frame variables.

$$
\hat{F}^b = \frac{d}{dt}(m\hat{v}^b)
$$

The above relationship can be re-written using the abstraction provided by the Transport Theorem. This will give the force acting on the body according to the inertial frame. The mass is treated as a constant in both frames. That leaves:

$$
\begin{align}
\hat{F}^b &= m(\hat{\dot{v}}^b + \hat{\omega} \times \hat{v}^b)
\end{align}
$$

The linear state variables ($\hat{r}^n$, $\hat{v}^b$) are determined primarily by the forces acting on the vehicle. Thus solving for acceleration $\hat{\dot{v}}^b$ and equating with the acceleration acting on the body frame $\hat{F}^b / m$ yields the rate of change of velocity $\hat{v}^b$ in the body frame.

$$
\begin{align}
\hat{T} + R_n^b \hat{F_g}^n &= m(\hat{\dot{v}}^b + \hat{\omega} \times \hat{v}^b)\
\hat{\dot{v}}^b &= \frac{\hat{T} + R_n^b \hat{F_g}^n}{m} - \hat{\omega} \times \hat{v}^b\
\begin{bmatrix} \dot{v}x \ \dot{v}_y \ \dot{v}_z \end{bmatrix} &= \begin{bmatrix} - \omega{y} v_{z} + \omega_{z} v_{y} + g \sin{\theta}\ \omega_{x} v_{z} - \omega_{z} v_{x} - g \sin{\phi} \cos{\theta}\ -\frac{T}{m} - \omega_{x} v_{y} + \omega_{y} v_{x} + g\cos{\phi} \cos{\theta} \end{bmatrix}
\end{align}
$$

Using $R_b^n$ and the current body frame velocity $\hat{v}^b$ gives the rate of change of position $r^n$ in the inertial frame.

$$
\begin{bmatrix} \dot{r}^n_x \ \dot{r}^n_y \ \dot{r}^n_z \end{bmatrix} = R_b^n \begin{bmatrix} v_x \ v_y \ v_z \end{bmatrix}
$$

Angular Equations of Motion

Newton's second law can be applied in angular mechanics too. Moment is the rate of change of angular momentum.

$$
\hat{\tau} = \frac{d}{dt}(I\hat{\omega})
$$

Where $I$ is the moment of inertia matrix, also known as angular mass.

Going from Linear to Angular Motion

Assume the force is acting on a point mass $m$ at some distance, $\hat{r}$, from the origin to cause a rotation about an axis of rotation. Only the component of force perpendicular to the position vector will cause rotation. This can be obtained by the cross product. Note that the component parallel to the position vector will cause linear acceleration. The infinitesimal displacement $d\hat{r}$ can be approximated by an infinitesimal arc length of a circle originating at the origin. Then, $d\hat{r}=$