1 of 30

Chapter 3

Kinematics and Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide1

2 of 30

Aircraft State Variables

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide2

% Goal of next two chapters is to write equations of motion in the form:

% xdot = f(x,u)

% where x is our state vector and u is our input vector

% What are states?

% What are inputs?

\begin{table}

\centering

\begin{tabular}{|c|l|}

\hline

Name & Description \\

\hline \hline

$p_n$ & Inertial north position of MAV expressed along $\mathbf{i}^i$ in $\mathcal{F}^i$\\ \hline

$p_e$ & Inertial east position of MAV expressed along $\mathbf{j}^i$ in $\mathcal{F}^i$\\ \hline

$p_d$ & Inertial down position of MAV expressed along $\mathbf{k}^i$ in $\mathcal{F}^i$\\ \hline

$u$ & Ground velocity expressed along $\mathbf{i}^b$ in $\mathcal{F}^b$\\ \hline

$v$ & Ground velocity expressed along $\mathbf{j}^b$ in $\mathcal{F}^b$\\ \hline

$w$ & Ground velocity expressed along $\mathbf{k}^b$ in $\mathcal{F}^b$\\ \hline

$\phi$ & Roll angle defined with respect to $\mathcal{F}^{v2}$\\ \hline

$\theta$ & Pitch angle defined with respect to $\mathcal{F}^{v1}$\\ \hline

$\psi$ & Heading (yaw) angle defined with respect to $\mathcal{F}^v$ \\ \hline

$p$ & Body angular (roll) rate expressed along $\mathbf{i}^b$ in $\mathcal{F}^b$ \\ \hline

$q$ & Body angular (pitch) rate expressed along $\mathbf{j}^b$ in $\mathcal{F}^b$ \\ \hline

$r$ & Body angular (yaw) rate expressed along $\mathbf{k}^b$ in $\mathcal{F}^b$ \\

\hline

\end{tabular}

\end{table}

3 of 30

Translational Kinematics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide3

4 of 30

Rotational Kinematics

Inverting gives:

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide4

\begin{pmatrix} p \\ q \\ r \end{pmatrix} &=

\underbrace{\begin{pmatrix} \dot{\phi} \\ 0 \\ 0

\end{pmatrix}}_{\dot{\phi} \text{~is defined in~} \mathcal{F}^b}

+ \underbrace{R_{v2}^{b}(\phi)

\begin{pmatrix} 0 \\ \dot{\theta} \\

0 \end{pmatrix}}_{\dot{\theta} \text{~is defined in~} \mathcal{F}^{v2}} + \underbrace{R_{v2}^{b}(\phi) R_{v1}^{v2}(\theta)

\begin{pmatrix} 0 \\ 0 \\ \dot{\psi} \end{pmatrix}}_{\dot{\psi} \text{~is defined in~} \mathcal{F}^{v1}} \notag \\

&= \begin{pmatrix} \dot{\phi} \\ 0 \\ 0 \end{pmatrix}

+ \begin{pmatrix}

1 & 0 & 0 \\

0 & \cos\phi & \sin\phi \\

0 & -\sin\phi & \cos\phi

\end{pmatrix}

\begin{pmatrix} 0 \\ \dot{\theta} \\ 0 \end{pmatrix}

+ \begin{pmatrix}

1 & 0 & 0 \\

0 & \cos\phi & \sin\phi \\

0 & -\sin\phi & \cos\phi

\end{pmatrix}

\begin{pmatrix}

\cos\theta & 0 & -\sin\theta \\

0 & 1 & 0 \\

\sin\theta & 0 & \cos\theta

\end{pmatrix}

\begin{pmatrix} 0 \\ 0 \\ \dot{\psi} \end{pmatrix} \notag \\

&= \begin{pmatrix}

1 & 0 & -\sin\theta \\

0 & \cos\phi & \sin\phi \cos\theta \\

0 & -\sin\phi & \cos\phi \cos\theta

\end{pmatrix}

\begin{pmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix}

= \begin{pmatrix}

1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\

0 & \cos\phi & -\sin\phi \\

0 & \sin\phi\sec\theta & \cos\phi\sec\theta

\end{pmatrix}

\begin{pmatrix} p \\ q \\ r

\end{pmatrix}

5 of 30

Kinematic Equations of Motion

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide5

\newcommand{\sth}{\text{s}_\theta}

\newcommand{\cth}{\text{c}_\theta}

\newcommand{\sph}{\text{s}_\phi}

\newcommand{\cph}{\text{c}_\phi}

\newcommand{\sps}{\text{s}_\psi}

\newcommand{\cps}{\text{c}_\psi}

�

\noindent Six of the 12 state equations for the MAV come from the kinematic \\

equations relating positions and velocities:

�

\begin{align*}

\begin{pmatrix} \dot{p}_n \\ \dot{p}_e \\ \dot{p}_d \end{pmatrix} &= \begin{pmatrix} \cth \cps & \sph \sth \cps - \cph \sps

& \cph \sth \cps + \sph \sps \\

\cth \sps & \sph \sth \sps + \cph \cps

& \cph \sth \sps - \sph \cps \\

-\sth & \sph \cth & \cph \cth

\end{pmatrix}

\begin{pmatrix} u \\ v \\ w \end{pmatrix}

\\

\begin{pmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix}

&= \begin{pmatrix}

1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\

0 & \cos\phi & -\sin\phi \\

0 & \sin\phi\sec\theta & \cos\phi\sec\theta

\end{pmatrix}

\begin{pmatrix} p \\ q \\ r

\end{pmatrix}

\end{align*}

�

\noindent

The remaining six equations will come from applying Newton’s 2nd law

to the translational and rotational motion of the aircraft

�

6 of 30

Differentiation of a Vector in Two Reference Frames

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide6

7 of 30

Differentiation of a Vector in Two Reference Frames

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide7

\par\noindent Recall from physics that for any vector $\mathbf{v}$ fixed in $\mathcal{F}^b$ we have

\[

\frac{d}{dt_i}\mathbf{v} = \boldsymbol{\omega}_{b/i} \times \mathbf{v}.

\]

Therefore

\begin{align*}

\frac{d}{dt_i}\mathbf{i}^b &= \boldsymbol{\omega}_{b/i}\times\mathbf{i}^b \\

\frac{d}{dt_i}\mathbf{j}^b &= \boldsymbol{\omega}_{b/i}\times\mathbf{j}^b \\

\frac{d}{dt_i}\mathbf{k}^b &= \boldsymbol{\omega}_{b/i}\times\mathbf{k}^b.

\end{align*}

Therefore

\begin{align*}

p_x \frac{d}{dt_i}\mathbf{i}^b + p_y \frac{d}{dt_i}\mathbf{j}^b + p_z \frac{d}{dt_i}\mathbf{k}^b

&= p_x \boldsymbol{\omega}_{b/i}\times\mathbf{i}^b

+ p_y \boldsymbol{\omega}_{b/i}\times\mathbf{j}^b

+ p_z \boldsymbol{\omega}_{b/i}\times\mathbf{k}^b

\\

&= \boldsymbol{\omega}_{b/i}\times\left(p_x\mathbf{i}^b + p_y\mathbf{j}^b + p_z\mathbf{k}^b\right) \\

&= \boldsymbol{\omega}_{b/i}\times\mathbf{p},

\end{align*}

resulting in

\[

\frac{d}{dt_i}\mathbf{p} = \frac{d}{dt_b}\mathbf{p} + \boldsymbol{\omega}_{b/i}

\times\mathbf{p}.

\]

8 of 30

Translational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide8

9 of 30

Translational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide9

\newcommand{\sth}{\text{s}_\theta}

\newcommand{\cth}{\text{c}_\theta}

\newcommand{\sph}{\text{s}_\phi}

\newcommand{\cph}{\text{c}_\phi}

\newcommand{\sps}{\text{s}_\psi}

\newcommand{\cps}{\text{c}_\psi}

\newcommand{\mass}{\mathsf{m}}

\newcommand{\Vbf}{\mathbf{V}}

�

\noindent Expressing

$\mass \left(\frac{d\mathbf{V}_g}{dt_b} +

\boldsymbol{\omega}_{b/i}\times\mathbf{V}_g \right) = \mathbf{f}$

in the body frame gives

\[

\mass \left(\frac{d\mathbf{V}^b_g}{dt_b} +

\boldsymbol{\omega}^b_{b/i}\times\mathbf{V}^b_g \right) = \mathbf{f}^b

\]

where

\[

\mathbf{V}^b_g = \begin{pmatrix} u \\ v \\ w

\end{pmatrix}

\qquad

\boldsymbol{\omega}^b_{b/i} = \begin{pmatrix} p \\ q \\ r

\end{pmatrix}

\qquad

\boldsymbol{f}^b = \begin{pmatrix} f_x \\ f_y \\ f_z

\end{pmatrix}

\]

Since

$\frac{d\mathbf{V}^b_g}{dt_b} = \begin{pmatrix} \dot{u} \\ \dot{v} \\ \dot{w}

\end{pmatrix}$

we have that

\[

\begin{pmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{pmatrix}

= -\begin{pmatrix} p \\ q \\ r \end{pmatrix}\times\begin{pmatrix} u \\ v \\ w \end{pmatrix} + \frac{1}{\mass} \begin{pmatrix} f_x \\ f_y \\ f_z \end{pmatrix}

= \begin{pmatrix} rv-qw \\ pw-ru \\ qu-pv \end{pmatrix} +

\frac{1}{\mass} \begin{pmatrix} f_x \\ f_y \\ f_z \end{pmatrix}

\]

�

10 of 30

Rotational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide10

\renewcommand{\textwidth}{3in}

\newcommand{\sth}{\text{s}_\theta}

\newcommand{\cth}{\text{c}_\theta}

\newcommand{\sph}{\text{s}_\phi}

\newcommand{\cph}{\text{c}_\phi}

\newcommand{\sps}{\text{s}_\psi}

\newcommand{\cps}{\text{c}_\psi}

\newcommand{\mass}{\mathsf{m}}

\newcommand{\Vbf}{\mathbf{V}}

\noindent Newton's 2nd Law:

\[

\frac{d\mathbf{h}}{dt_i} = \mathbf{m}

\]

\begin{itemize}

\item $\mathbf{h}$ is the angular momentum vector

\item $\mathbf{m}$ is the sum of all external moments

\item Time derivative taken wrt inertial frame

\end{itemize}

\noindent Therefore we have

\[

\frac{d\mathbf{h}}{dt_i}

= \frac{d\mathbf{h}}{dt_b} + \boldsymbol{\omega}_{b/i}\times\mathbf{h} = \mathbf{m}.

\]

Expressing in the body frame gives

\[

\frac{d\mathbf{h}^b}{dt_b} + \boldsymbol{\omega}^b_{b/i}\times\mathbf{h}^b

= \mathbf{m}^b.

\]

11 of 30

Rotational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide11

\newcommand{\sth}{\text{s}_\theta}

\newcommand{\cth}{\text{c}_\theta}

\newcommand{\sph}{\text{s}_\phi}

\newcommand{\cph}{\text{c}_\phi}

\newcommand{\sps}{\text{s}_\psi}

\newcommand{\cps}{\text{c}_\psi}

\newcommand{\mass}{\mathsf{m}}

\newcommand{\Vbf}{\mathbf{V}}

%\newcommand{\defeq}{\stackrel{\triangle}{=}}

�

\noindent For a rigid body, angular momentum is defined as the product of the inertia

matrix and the angular velocity vector:

\[

\mathbf{h}^b \defeq \mathbf{J}\boldsymbol{\omega}_{b/i}^b

\]

\noindent where

\begin{align*}

\mathbf{J} &=

\begin{pmatrix}

\int (y^2 + z^2)\,d\mass & -\int xy\,d\mass & -\int xz\,d\mass \\

-\int xy\,d\mass & \int (x^2 + z^2)\,d\mass & -\int yz\,d\mass \\

-\int xz\,d\mass & -\int yz\,d\mass & \int (x^2 + y^2)\,d\mass

\end{pmatrix} \label{eq:kinematics-J}\\

&\defeq

\begin{pmatrix}

J_{x} & -J_{xy} & -J_{xz} \\

-J_{xy} & J_{y} & -J_{yz} \\

-J_{xz} & -J_{yz} & J_{z}

\end{pmatrix}

\end{align*}

�

\noindent Diagonal elements are called moments of inertia. Off-diagonal

elements are called products of inertia

�

\vspace{.5cm}

�

\noindent $\mathbf{J}$ determined from mass properties in CAD program or measured

experimentally using a bifilar pendulum

12 of 30

Rotational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide12

13 of 30

Rotational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide13

\newcommand{\defeq}{\stackrel{\triangle}{=}}

\noindent If the aircraft is symmetric about the $\mathbf{i}^b\text{-}\mathbf{k}^b$ plane, then $J_{xy}=J_{yz}=0$ and

\[

\mathbf{J} =

\begin{pmatrix}

J_{x} & 0 & -J_{xz} \\

0 & J_{y} & 0 \\

-J_{xz} & 0 & J_{z}

\end{pmatrix}

\]

\noindent This symmetry assumption helps to simplify the analysis. The inverse of $\mathbf{J}$ becomes

\begin{align*}

\mathbf{J}^{-1}

&= \frac{\text{adj}(\mathbf{J})}{\text{det}(\mathbf{J})}

= \frac{\begin{pmatrix}

J_y J_z & 0 & J_y J_{xz} \\

0 & J_xJ_z-J_{xz}^2 & 0 \\

J_{xz}J_y & 0 & J_x J_y

\end{pmatrix}}

{ J_x J_y J_z - J_{xz}^2 J_y } \\

&= \begin{pmatrix}

\frac{J_z}{\Gamma} & 0 & \frac{J_{xz}}{\Gamma} \\

0 & \frac{1}{J_y} & 0 \\

\frac{J_{xz}}{\Gamma} & 0 & \frac{J_{x}}{\Gamma}

\end{pmatrix}

\end{align*}

where

\[

\Gamma \defeq J_xJ_z - J_{xz}^2

\]

14 of 30

Rotational Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide14

%\newcommand{\defeq}{\stackrel{\triangle}{=}}

�

\[

\textrm{Define }\mathbf{m}^b \defeq \begin{pmatrix} l \\ m \\ n \end{pmatrix} \quad

\textrm{and recall that} \quad \mathbf{a} \times \mathbf{b} =

\begin{pmatrix}

0 & -a_3 & a_2 \\

a_3 & 0 & -a_1 \\

-a_2 & a_1 & 0

\end{pmatrix}

\begin{pmatrix} b_1 \\ b_2 \\ b_3 \end{pmatrix}

\]

Then

\[

\dot{\boldsymbol{\omega}}_{b/i}^b = \mathbf{J}^{-1}\left[

-\boldsymbol{\omega}_{b/i}^b\times\left(\mathbf{J}\boldsymbol{\omega}_{b/i}^b\right)

+ \mathbf{m}^b \right]

\]

can be expressed as

\begin{align*}

\begin{pmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{pmatrix}

&= \begin{pmatrix}

\frac{J_z}{\Gamma} & 0 & \frac{J_{xz}}{\Gamma} \\

0 & \frac{1}{J_y} & 0 \\

\frac{J_{xz}}{\Gamma} & 0 & \frac{J_{x}}{\Gamma}

\end{pmatrix}

\left[

\begin{pmatrix}

0 & r & -q \\

-r & 0 & p \\

q & -p & 0

\end{pmatrix}

\begin{pmatrix}

J_{x} & 0 & -J_{xz} \\

0 & J_{y} & 0 \\

-J_{xz} & 0 & J_{z}

\end{pmatrix}

\begin{pmatrix} p \\ q \\ r \end{pmatrix}

+ \begin{pmatrix} l \\ m \\ n \end{pmatrix}

\right] \nonumber \\

&= \begin{pmatrix}

\frac{J_z}{\Gamma} & 0 & \frac{J_{xz}}{\Gamma} \\

0 & \frac{1}{J_y} & 0 \\

\frac{J_{xz}}{\Gamma} & 0 & \frac{J_{x}}{\Gamma}

\end{pmatrix}

\left[

\begin{pmatrix}

J_{xz}pq + (J_y-J_z)qr \\

J_{xz}(r^2-p^2) + (J_z-J_x)pr \\

(J_x-J_y)pq - J_{xz}qr

\end{pmatrix}

+ \begin{pmatrix} l \\ m \\ n \end{pmatrix}

\right] \nonumber \\

&= \begin{pmatrix}

\Gamma_1 pq - \Gamma_2 qr + \Gamma_3 l + \Gamma_4 n \\

\Gamma_5 pr - \Gamma_6 (p^2-r^2) + \frac{1}{J_y} m \\

\Gamma_7 pq - \Gamma_1 qr + \Gamma_4 l + \Gamma_8 n

\end{pmatrix}

\end{align*}

where $\Gamma$'s are functions of moments and products of inertia

�

15 of 30

Equation of Motion Summary

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide15

\newcommand{\sth}{\text{s}_\theta}

\newcommand{\cth}{\text{c}_\theta}

\newcommand{\sph}{\text{s}_\phi}

\newcommand{\cph}{\text{c}_\phi}

\newcommand{\sps}{\text{s}_\psi}

\newcommand{\cps}{\text{c}_\psi}

%\newcommand{\mass}{\mathsf{m}}

\newcommand{\Vbf}{\mathbf{V}}

%\newcommand{\defeq}{\stackrel{\triangle}{=}}

�

\noindent The equations of motion are a system of 12 first-order ODE’s:

\begin{align*}

\begin{pmatrix} \dot{p}_n \\ \dot{p}_e \\ \dot{p}_d \end{pmatrix}

&= \begin{pmatrix} \cth \cps & \sph \sth \cps - \cph \sps

& \cph \sth \cps + \sph \sps \\

\cth \sps & \sph \sth \sps + \cph \cps

& \cph \sth \sps - \sph \cps \\

-\sth & \sph \cth & \cph \cth

\end{pmatrix}

\begin{pmatrix} u \\ v \\ w \end{pmatrix}

\label{eq:kin-eom-xyh} \\

\begin{pmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{pmatrix}

&= \begin{pmatrix} rv-qw \\ pw-ru \\ qu-pv \end{pmatrix} +

\frac{1}{\mass} \begin{pmatrix} f_x \\ f_y \\ f_z \end{pmatrix}

\\

\begin{pmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{pmatrix}

& = \begin{pmatrix}

1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\

0 & \cos\phi & -\sin\phi \\

0 & \frac{\sin\phi}{\cos\theta} & \frac{\cos\phi}{\cos\theta}

\end{pmatrix}

\begin{pmatrix} p \\ q \\ r \end{pmatrix} \\

\begin{pmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{pmatrix}

&= \begin{pmatrix}

\Gamma_1 pq - \Gamma_2 qr \\

\Gamma_5 pr - \Gamma_6 (p^2-r^2) \\

\Gamma_7 pq - \Gamma_1 qr

\end{pmatrix}

+\begin{pmatrix}

\Gamma_3 l + \Gamma_4 n \\

\frac{1}{J_y} m \\

\Gamma_4 l + \Gamma_8 n

\end{pmatrix}

\end{align*}

--

where

\begin{align*}

\Gamma_1 &= \frac{J_{xz}(J_x - J_y + J_z)}{\Gamma} \\

\Gamma_2 &= \frac{J_z(J_z-J_y) + J_{xz}^2}{\Gamma} \\

\Gamma_3 &= \frac{J_z}{\Gamma} \\

\Gamma_4 &= \frac{J_{xz}}{\Gamma} \\

\Gamma_5 &= \frac{J_z-J_x}{J_y} \\

\Gamma_6 &= \frac{J_{xz}}{J_y} \\

\Gamma_7 &= \frac{(J_x-J_y)J_x+ J_{xz}^2}{\Gamma} \\

\Gamma_8 &= \frac{J_{x}}{\Gamma} \\

\Gamma &= J_xJ_z - J_{xz}^2

\end{align*}

�

16 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide16

17 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide17

18 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide18

19 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide19

{\bf \hspace{0.3in} Conversion Between Euler Angles and Quaternions}

\begin{align*}

\phi &= \text{atan2} \left( 2(e_0e_1 + e_2e_3), (e_0^2+e_3^2-e_1^2-e_2^2) \right) \\

%\theta &= -\frac{\pi}{2} + \text{atan2}\left(\sqrt{1+2(e_0e_2-e_1e_3)}, \sqrt{1-2(e_0e_2-e_1e_3)}\right) \\

\theta &= \text{asin} \left( 2(e_0e_2 - e_1e_3) \right) \\

\psi &= \text{atan2} \left( 2(e_0e_3 + e_1e_2), (e_0^2+e_1^2-e_2^2-e_3^2) \right)

\end{align*}

�

\noindent

From the yaw, pitch, and roll Euler angles ($\psi$, $\phi$, $\theta$), the corresponding quaternion elements are

\begin{align*}

e_0 &= \cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2}+\sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\

e_1 &= \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}-\sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} \\

e_2 &= \cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2}+\sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} \\

e_3 &= \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2}-\cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2}

\end{align*}

20 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide20

21 of 30

Quaternion Kinematics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide21

22 of 30

Equation of Motion Summary

Quaternion EOM are simpler (linear)
Require conversion to Euler angles
Must be normalized after each integration step

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide22

\newcommand{\mass}{\mathsf{m}}

\newcommand{\Vbf}{\mathbf{V}}

\newcommand{\defeq}{\stackrel{\triangle}{=}}

�

\noindent The equations of motion are a system of 13 first-order ODE’s:

\begin{align*}

\begin{pmatrix} \dot{p}_n \\ \dot{p}_e \\ \dot{p}_d \end{pmatrix}

&=

\begin{pmatrix}

e_1^2+e_0^2-e_2^2-e_3^2 & 2(e_1e_2-e_3e_0) & 2(e_1e_3+e_2e_0) \\

2(e_1e_2+e_3e_0) & e_2^2+e_0^2-e_1^2-e_3^2 & 2(e_2e_3-e_1e_0) \\

2(e_1e_3-e_2e_0) & 2(e_2e_3+e_1e_0) & e_3^2+e_0^2-e_1^2-e_2^2

\end{pmatrix}

\begin{pmatrix} u \\ v \\ w \end{pmatrix}

\label{eq:kin-eom-xyh} \\

\begin{pmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{pmatrix}

&= \begin{pmatrix} rv-qw \\ pw-ru \\ qu-pv \end{pmatrix} +

\frac{1}{\mass} \begin{pmatrix} f_x \\ f_y \\ f_z \end{pmatrix}

\\

\begin{pmatrix} \dot{e}_0 \\ \dot{e}_1 \\ \dot{e}_2 \\ \dot{e}_3 \end{pmatrix}

&= \frac{1}{2} \begin{pmatrix}

0 & -p & -q & -r \\

p & 0 & r & -q \\

q & -r & 0 & p \\

r & q & -p & 0

\end{pmatrix}\begin{pmatrix} e_0 \\ e_1 \\ e_2 \\ e_3 \end{pmatrix}

\\

\begin{pmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{pmatrix}

&= \begin{pmatrix}

\Gamma_1 pq - \Gamma_2 qr \\

\Gamma_5 pr - \Gamma_6 (p^2-r^2) \\

\Gamma_7 pq - \Gamma_1 qr

\end{pmatrix}

+\begin{pmatrix}

\Gamma_3 l + \Gamma_4 n \\

\frac{1}{J_y} m \\

\Gamma_4 l + \Gamma_8 n

\end{pmatrix}

\end{align*}

where

\begin{alignat*}{5}

\Gamma_1 &= \frac{J_{xz}(J_x - J_y + J_z)}{\Gamma} & \qquad

\Gamma_2 &= \frac{J_z(J_z-J_y) + J_{xz}^2}{\Gamma} & \qquad

\Gamma_3 &= \frac{J_z}{\Gamma} \\

\Gamma_4 &= \frac{J_{xz}}{\Gamma} & \qquad

\Gamma_5 &= \frac{J_z-J_x}{J_y} & \qquad

\Gamma_6 &= \frac{J_{xz}}{J_y} \\

\Gamma_7 &= \frac{(J_x-J_y)J_x + J_{xz}^2}{\Gamma} & \qquad

\Gamma_8 &= \frac{J_{x}}{\Gamma} & \qquad

\Gamma &= J_xJ_z - J_{xz}^2

\end{alignat*}

23 of 30

Quaternions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide23

\par\noindent In Python, $e$ needs to be normalized after applying the RK4 update step to ensure that $\|e\|=1$

�

\vspace{0.5cm}

\noindent

In Simulink, we can ensure that $\|e\|\approx 1$ by appending the quaternion kinematics as

\begin{align*}

\begin{pmatrix} \dot{e}_0 \\ \dot{e}_1 \\ \dot{e}_2 \\ \dot{e}_3 \end{pmatrix}

&= \frac{1}{2} \begin{pmatrix}

0 & -p & -q & -r \\

p & 0 & r & -q \\

q & -r & 0 & p \\

r & q & -p & 0

\end{pmatrix}\begin{pmatrix} e_0 \\ e_1 \\ e_2 \\ e_3 \end{pmatrix} - \lambda\frac{\partial J}{\partial e} \\

&= \frac{1}{2} \begin{pmatrix}

\lambda (1-\|e\|^2) & -p & -q & -r \\

p & \lambda (1-\|e\|^2) & r & -q \\

q & -r & \lambda (1-\|e\|^2) & p \\

r & q & -p & \lambda (1-\|e\|^2)

\end{pmatrix} \begin{pmatrix} e_0 \\ e_1 \\ e_2 \\ e_3 \end{pmatrix}

\end{align*}

where $J=\frac{1}{8}(1-\|e\|^2)^2$ and where $\lambda>0$. The second term forces $\|e\|\to 1$. In our experience, a value of $\lambda=1000$ seems to work well, but a stiff solver like ODE15s must be used

�

24 of 30

Runge-Kutta Integration

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide24

25 of 30

RK1 Algorithm

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide25

26 of 30

RK2 Algorithm

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide26

\setlength{\textwidth}{10in}

�

\noindent To improve accuracy, the integral can be approximated using the trapezoidal rule

\[

\int_a^b f(\xi)d\xi \approx (b-a)\left(\frac{f(a)+f(b)}{2}\right)

\]

�

\noindent Accordingly,

\[

\int_{t-T_s}^t f(x(\tau), u(\tau))d\tau \approx \frac{T_s}{2}\big[f(x(t-T_s),u(t-T_s)) + f(x(t), u(t))\big]

\]

$x(t)$ is unknown so use RK1 to approximate it as

\begin{multline*}

\int_{t-T_s}^t f(x(\tau), u(\tau))d\tau \approx \frac{T_s}{2}\Big[f(x(t-T_s),u(t-T_s))

\\ + f(x(t-T_s) + T_s f(x(t-T_s), u(t-T_s)), u(t-T_s))\Big]

\end{multline*}

�

\noindent The numerical integration routine can be written as

\begin{align*}

x_0 &= x(t_0) \\

X_1 &= f(x_{k-1}, u_{k-1}) \\

X_2 & = f(x_{k-1} + T_sX_1, u_{k-1}) \\

x_k &= x_{k-1} + \frac{T_s}{2} \left( X_1 + X_2 \right)

\end{align*}

�

\noindent This is the Runge-Kutta second-order method or RK2

27 of 30

RK4 Algorithm

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide27

28 of 30

RK4 Algorithm

Adapted from HilberTraum - Own work, CC BY-SA 4.0, https://commons.wikimedia.org/w/index.php?curid=64366870

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide28

29 of 30

DYNAMICS AND RK4 IMPLEMENTATION

See example code

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide29

30 of 30

Project

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide30

\begin{enumerate}

\item

Implement the MAV equations of motion given in Equations~(3.14) through (3.17). Assume that the inputs to the block are the forces and moments applied to the MAV in the body frame. Changeable parameters should include the mass, the moments and products of inertia, and the initial conditions for each state. Use the parameters given in Appendix~E.

�

\item Verify that the equations of motion are correct by individually setting the forces and moments along each axis to a nonzero value and convincing yourself that the motion is appropriate.

�

\item

Since $J_{xz}$ is non-zero, there is gyroscopic coupling between roll and yaw. To test your simulation, set $J_{xz}$ to zero and

place nonzero moments on $l$ and $n$ and verify that there is no coupling between the roll and yaw axes. Verify that when $J_{xz}$ is not zero, there is coupling between the roll and yaw axes.

\end{enumerate}