Quadcopters exhibit a property called Differential Flatness. A system that has Differential Flatness allows you to solve for some of the state variables in terms of other state variables. In the case of a quadcopter, this means that if you give me a trajectory \(x(t)\), \(y(t)\), \(z(t)\), and \(\psi(t)\) of a quadcopter, I can tell you what the other state variables of the quadcopter are at every point in the trajectory. The only caveat for quadcopters is that the trajectory must be differentiable 4 times at every given point. If you haven’t encountered differential flatness before, see section 10.8.1 of [1] for a quick overview.
As shown in section 2.3.2 of [2], let \(\sigma\) be our flat outputs which we assume are known,
$$
\sigma =
\begin{bmatrix}
\sigma_1 \\ \sigma_2 \\ \sigma_3 \\ \sigma_4
\end{bmatrix} =
\begin{bmatrix}
x \\ y \\ z \\ \psi
\end{bmatrix}
$$
Note that we also assume we know \(\dot \sigma\), \(\ddot \sigma\), \(\dddot \sigma\), and \(\ddddot \sigma\). With these assumptions, we want to solve for \(\phi\), \(\dot \phi\), \(\ddot \phi\), \(\theta\), \(\dot \theta\), and \(\ddot \theta\).
Solving for \(\phi\) and \(\theta\)
To start let,
$$
\textbf{z}_B = \frac{\textbf{t}}{\lVert \textbf{t} \rVert}
$$
where,
$$
\textbf{t} =
\begin{bmatrix}
\ddot x \\
\ddot y \\
\ddot z + g
\end{bmatrix}
$$
and \(g\) is gravity. Additionally, let
$$
\textbf{x}_C =
\begin{bmatrix}
\cos(\psi) \\ \sin(\psi) \\ 0
\end{bmatrix}
$$
and
$$
\textbf{y}_B = \frac{\textbf{z}_B \times \textbf{x}_C}{\lVert \textbf{z}_B \times \textbf{x}_C \rVert}
$$
and
$$
\textbf{x}_B = \textbf{y}_B \times \textbf{z}_B
$$
This allows us to define,
$$
R_B^W = \begin{bmatrix} \textbf{x}_B & \textbf{y}_B & \textbf{z}_B \end{bmatrix}
$$
which can be used to solve for \(\phi\) and \(\theta\) by noting that,
$$
R_B^W = \begin{bmatrix}
\cos(\psi) \cos(\theta) - \sin(\phi) \sin(\psi) \sin(\theta) & -\cos(\phi) \sin(\psi) & \cos(\psi) \sin(\theta) + \cos(\theta) \sin(\phi) \sin(\psi) \\
\cos(\theta) \sin(\psi) + \cos(\psi) \sin(\phi) \sin(\theta) & \cos(\phi) \cos(\psi) & \sin(\psi) \sin(\theta) - \cos(\psi) \cos(\theta) \sin(\phi) \\
-\cos(\phi) \sin(\theta) & \sin(\phi) & \cos(\phi) \cos(\theta)
\end{bmatrix}
$$
from the definition of Z - X - Y Euler angles. Therefore,
$$
\begin{align}
\phi & = \sin^{-1}(\textbf{y}_{B_3}) \\
\theta & = \cos^{-1}(\frac{\textbf{z}_{B_3}}{\cos(\phi)})
\end{align}
$$
where \(\textbf{y}_{B_3}\) denotes the third element of the \(\textbf{y}_B\) vector and \(\textbf{z}_{B_3}\) denotes the third element of the \(\textbf{z}_B\) vector.
Solving for \(\dot \phi\) and \(\dot \theta\)
We can also define,
$$
\textbf{h}_\omega = \frac{m}{u_1} (\dot{\textbf{a}} - (\textbf{z}_B \cdot \dot{\textbf{a}})\textbf{z}_B)
$$
where \(u_1\) is the total thrust and,
$$
\dot{\textbf{a}} = \begin{bmatrix} \dddot x \\ \dddot y \\ \dddot z \end{bmatrix}
$$
commonly referred to as Jerk.
This allows us to define,
$$
\begin{align}
p & = -\textbf{h}_\omega \cdot \textbf{y}_B \\
q & = \textbf{h}_\omega \cdot \textbf{x}_B \\
r & = \dot \psi \textbf{z}_W \cdot \textbf{z}_B
\end{align}
$$
where \(\textbf{z}_W\) is defined as the unit vector pointing in the direction of the z-axis in the world frame.
We can then use the following equation to get \(\dot \phi\) and \(\dot \theta\),
$$
\begin{bmatrix}\dot \phi \\ \dot \theta \\ \dot \psi \end{bmatrix} =
\begin{bmatrix} \textbf{x}_C & \textbf{y}_B & \textbf{z}_W \end{bmatrix}^{-1} \textbf{R}_B^W \begin{bmatrix}p \\ q \\r \end{bmatrix}
$$
Solving for \(\ddot \phi\) and \(\ddot \theta\)
To solve for \(\ddot \phi\) and \(\ddot \theta\), let
$$
\begin{align}
\omega_{BW} & = \begin{bmatrix} \textbf{x}_C & \textbf{y}_C & \textbf{z}_W \end{bmatrix} \begin{bmatrix} \dot \phi \\ \dot \theta \\ \dot \psi \end{bmatrix}\\
\omega_{CW} & = \dot \psi \textbf{z}_W
\end{align}
$$
as well as,
$$
\begin{align}
\dot u_1 & = \textbf{z}_B \cdot m \dot{\textbf{a}} \\
\ddot u_1 & = \textbf{z}_B \cdot m \ddot{\textbf{a}} - \textbf{z}_B \cdot (\omega_{BW} \times \omega_{BW} \times u_1 \textbf{z}_B) \\
\textbf{h}_{\alpha} & = \alpha_{BW} \times \textbf{z}_B = \frac{1}{u_1}(m \ddot{\textbf{a}} - \ddot u_1 \textbf{z}_B - 2 \omega_{BW} \times \dot u_1 \textbf{z}_B - \omega_{BW} \times \omega_{BW} \times u_1 \textbf{z}_B)
\end{align}
$$
where,
$$
\ddot{\textbf{a}} = \begin{bmatrix} \ddddot x \\ \ddddot y \\ \ddddot z \end{bmatrix}
$$
commonly referred to as Snap.
Now we can define,
$$
\begin{align}
\dot p & = -\textbf{h}_{\alpha} \cdot \textbf{y}_B \\
\dot q & = \textbf{h}_{\alpha} \cdot \textbf{x}_B
\end{align}
$$
Finally, we can solve for \(\dot r\) using the third equation from the following nasty system of equations,
$$
R_B^W (\begin{bmatrix} \dot p \\ \dot q \\ \dot r \end{bmatrix} + \begin{bmatrix} p \\ q \\ r \end{bmatrix} \times \begin{bmatrix} p \\ q \\ r \end{bmatrix}) =
\omega_{CW} \times \dot \phi \textbf{x}_c + \omega_{BW} \times \dot \theta \textbf{y}_B + \begin{bmatrix} \textbf{x}_C & \textbf{y}_B & \textbf{z}_W \end{bmatrix} \begin{bmatrix} \ddot \phi \\ \ddot \theta \\ \ddot \psi \end{bmatrix}
$$
Remember that we are given \(\ddot \psi\) which is what allows us to solve for \(\dot r\).
See section 2.3.2 of [2] and section \(III\) of [3] for a more in-depth derivation of the equations shown in this post.
Summary
With these equations in hand, we are now able to determine the entire state of a quadcopter knowing only \(x\), \(y\), \(z\), \(\psi\) and their derivatives with respect to time (each differentiated 4 times). The usefulness of these equations will be clear in the next post where we do motion planning. See you there.
References
[1] Russ Tedrake. Underactuated Robotics: Algorithms for Walking, Running, Swimming, Flying, and Manipulation (Course Notes for MIT 6.832). Downloaded on 6/4/22 from http://underactuated.mit.edu/
[2] Mellinger, Daniel Warren, "Trajectory Generation and Control for Quadrotors" (2012). Publicly Accessible Penn Dissertations. 547. https://repository.upenn.edu/edissertations/547
[3] D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 2520-2525, doi: 10.1109/ICRA.2011.5980409. https://ieeexplore.ieee.org/document/5980409