# Quadrotor Model

Multirotors are underactuated aerial robots that consist of multiple motor/propeller combinations. A quadrotor has four motor/propellor combinations, as shown in the below figure.

Figure

## Coordinate Frames

Coordinate frames are rigid, orthonormal bases of $\mathbb{R}^3$. There are two main coordinate frames of interest: the inertial frame $\mathcal{F}^i$ and the body frame $\mathcal{F}^b$. The inertial frame $\mathcal{F}^i$ is an orthonormal basis of $\mathbb{R}^3$ that is earth-fixed with an origin defined as the ''home'' position. The body frame $\mathcal{F}^b$ represents the quadrotor and we assume that its origin is the center of mass of the quadrotor. 

Intermediate frames exist that help us account for the roll, pitch, and yaw rotations of the quadrotor. The first intermediate frame is the vehicle frame $\mathcal{F}^v$, which is at the position of the unrolled, unpitched, unyawed quadrotor. The vehicle frame leads to the vehicle-1 frame $\mathcal{F}^{v1}$ by yawing about the $\mathbf{k}^v$ axis, resulting in the unrolled, unpitched frame. Pitching about the new $\mathbf{j}^{v1}$ axis results in the unrolled vehicle-2 frame $\mathcal{F}^{v2}$, and rolling about the new $\mathbf{i}^{v2}$ axis results in the properly oriented body frame $\mathcal{F}^b$.

Additional frames may be the camera frame, the gimbal frame, or other sensor or measurement frames.

Note that in this work, we use the aerospace standard of passive, right-handed rotations and we will use 3-2-1 ($\psi$-$\theta$-$\phi$) Euler angles for yaw, pitch, roll, respectively.

Python code that builds proper rotation matrices in $\mathbb{R}^{3\times3}$ is given below. These rotation matrices can be used to rotate from the vehicle (or inertial) frame to the body frame through all the intermediate frames.


```python
import numpy as np

def Rot_v_to_v1(psi):
    R = np.array([
        [ np.cos(psi), np.sin(psi), 0],
        [-np.sin(psi), np.cos(psi), 0],
        [     0,           0    ,   1]
    ])
    return R

def Rot_v1_to_v2(theta):
    R = np.array([
        [np.cos(theta), 0, -np.sin(theta)],
        [      0      , 1,        0      ],
        [np.sin(theta), 0,  np.cos(theta)]
    ])
    return R

def Rot_v2_to_b(phi):
    R = np.array([
        [1,       0,           0     ],
        [0,  np.cos(phi), np.sin(phi)],
        [0, -np.sin(phi), np.cos(phi)]
    ])
    return R

def Rot_v_to_b(phi, theta, psi):
    return Rot_v2_to_b(phi).dot(Rot_v1_to_v2(theta).dot(Rot_v_to_v1(psi)))

def Rot_i_to_b(phi, theta, psi):
    return Rot_v_to_b(phi, theta, psi)
```

## Configuration Space

The quadrotor equations of motion consist of $6$ degrees of freedom (DOF) and $12$ ordinary differential equations ($6$ are kinematic and $6$ are dynamic). The quadrotor is modelled as a freely-rotating rigid body evolving in $\mathcal{M} = \mathbb{R}^3 \times \text{SO}(3) = \text{SE}(3)$.

Note that $\text{SO}(3)$ is a matrix Lie group that describes the orientation of rigid body in 3D space and is not isomorphic to $\mathbb{R}^3$. However, because Lie groups are smooth (i.e., differentiable) manifolds, there is an atlas of charts (i.e., homemorphisms) that allow the manifold to be locally described as a Euclidean space. A popular set of local parameterizations are the intuitive Euler angles, of which there are twelve. We will use the intrinsic 3-2-1 ($\psi$-$\theta$-$\phi$) Euler angle representation as is common in aerospace with first a yaw rotation about $\mathbf{k}$, a pitch rotation about $\mathbf{j}$, and then a roll rotation about $\mathbf{i}$. Additionally, we assume that the roll, pitch, and yaw of the quadrotor are small -- a logical assumption for the smooth trajectories of typical maneuvers. More precisely, we have that
$$
\phi \in [0, 2\pi) \subset \mathbb{R} \\
\theta \in (-\pi/2, \pi/2) \subset \mathbb{R} \\
\psi \in [0, 2\pi) \subset \mathbb{R}.
$$
Thus, the state of the quadrotor is given by $\mathbf{x} = \begin{bmatrix} x & y & z & \phi & \theta & \psi \end{bmatrix}^\top \in \mathbb{R}^6$.

## Equations of Motion

### Kinematics

Kinematics describe the motion of an object without consideration of what caused that motion. Using kinematics, we can relate position to velocity and orientation to angular velocity. Notice that these equations are irrespective of intertial properties such mass and size.

#### Translational

$$
\dot{\mathbf{r}}^i = R_b^i \mathbf{v}^b
$$

#### Rotational

When determining the rotational kinematic equation of a rigid body, we are asking how the angular velocities relate to the current orientation. A rate gyro is used to measure the angular velocity about each of the body-frame axes of the rigid body. These gyro measurements are given by $\mathbf{\omega}^b = p\mathbf{i}^b + q\mathbf{j}^b + r\mathbf{k}^b = \begin{bmatrix} p & q & r\end{bmatrix}^\top$.

Difficulty relating $\mathbf{\omega}$ with the orientation expressed with Euler angles arises because each Euler angle is defined in a different frame. Therefore, the derivatives of each Euler angle must be transformed into the proper frame as follows:

$$
\begin{align}
\mathbf{\omega} =
  \begin{bmatrix} p \\ q \\ r \end{bmatrix} &= 
  \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix} +
  R_{v2}^b(\phi) \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix} +
  R_{v2}^b(\phi) R_{v1}^{v2}(\theta) \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix} \\
&=
  \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix} +
  \begin{bmatrix}
    1 & 0 & 0 \\
    0 & \cos\phi & \sin\phi \\
    0 & -\sin\phi & \cos\phi
  \end{bmatrix}
  \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix} +
  \begin{bmatrix}
    1 & 0 & 0 \\
    0 & \cos\phi & \sin\phi \\
    0 & -\sin\phi & \cos\phi
  \end{bmatrix}
  \begin{bmatrix}
    \cos\theta & 0 & -\sin\theta \\
    0 & 1 & 0 \\
    \sin\theta & 0 & \cos\theta
  \end{bmatrix}
  \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix} \\
&=
\underbrace{
  \begin{bmatrix}
    1 & 0 & -\sin\theta \\
    0 & \cos\phi & \sin\phi\cos\theta \\
    0 & -\sin\phi & \cos\phi\cos\theta
  \end{bmatrix}
}_{\Gamma^{-1}}
  \begin{bmatrix} \dot\phi \\ \dot\theta \\ \dot\psi \end{bmatrix}.
\end{align}
$$

Note that $\det\Gamma^{-1} = cos\theta$. Thus, for $\theta \in (-\pi/2, \pi/2)$ the matrix $\Gamma^{-1}$ is invertible, and we can write the rotational kinematic equation using Euler angles as

$$
\mathbf{\dot\Phi} =
\begin{bmatrix} \dot\phi \\ \dot\theta \\ \dot\psi \end{bmatrix} =
\begin{bmatrix}
  1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\
  0 & \cos\phi & -\sin\phi \\
  0 & \sin\phi\sec\theta & \cos\phi\sec\theta
\end{bmatrix}
\begin{bmatrix} p \\ q \\ r \end{bmatrix} =
\Gamma(\phi,\theta) \mathbf{\omega}.
$$

### Dynamics

Dynamics describe the motion of objects in relation to forces and torques, which cause motion. This is often how we use a controller to interact with a physical system. This is because we can control actuators which cause a force or torque on the system.

#### Translational

It is easiest to express Newton's second law in the body frame. However, Newton's laws only hold in inertial frames. Therefore, we use the equation of Coriolis to express the derivative of an inertial frame quantity to the derivative in the body frame, which gives us

$$
\mathbf{F}_g - \mathbf{T} = m\dot{\mathbf{v}}^b + \mathbf{\omega} \times \mathbf{v}^b,
$$

where $\mathbf{F}_g = mg \begin{bmatrix} -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix}^\top$ and $\mathbf{T} = \begin{bmatrix} 0 & 0 & f \end{bmatrix}^\top$. This can be rearranged into

$$
\dot{\mathbf{v}}^b = \frac{1}{m}[\mathbf{F}_g - \mathbf{T} - \mathbf{\omega} \times \mathbf{v}^b].
$$

#### Translational (inertial)

Alternatively, we can express the translational dynamics in the inertial frame, which can be advantageous if we are trying to command inertial positions or velocities.

$$
m\ddot{\mathbf{r}}^i = mg\mathbf{k}^i - R_b^i \begin{bmatrix} 0 \\ 0 \\ u_1 \end{bmatrix}
$$

#### Translational (inertial) with Drag

We make the model more realistic by adding drag, see [3]. It is ideal to have a higher fidelity plant model than the control/estimation models---this will certainly be true in real life.

$$
m\ddot{\mathbf{r}}^i = mg\mathbf{k}^i -
R_b^i \begin{bmatrix} 0 \\ 0 \\ u_1 \end{bmatrix} -
\begin{bmatrix} \mu \\ \mu \\ 0 \end{bmatrix} \dot{\mathbf{r}}^i
$$

#### Rotational

Following Netwton's second law for rotational motion ($M = I\dot\omega$), we get

$$
I\mathbf{\dot\omega} = -\omega \times I\omega + M.
$$

We assume that the quadrotor is symmetric about its axes so that
$$
I=
\begin{bmatrix}
    I_x &  0  &  0  \\
     0  & I_y &  0  \\
     0  &  0  & I_z \\
\end{bmatrix}
$$

## Forces and Moments

Given that we have a dynamic model of a quadrotor, we must determine the forces and moments that are used to drive the 6 DOF dynamic model we have derived.

$$
\begin{bmatrix} u_1 \\ u_2 \\ u_3 \\ u_4 \end{bmatrix} =
\begin{bmatrix}
    1 & 1 & 1 & 1 \\
    0 & -\ell & 0 & \ell \\
    \ell & 0 & -\ell & 0 \\
    -c_T & c_T & -c_T & c_T \\
\end{bmatrix}
\begin{bmatrix} T_1 \\ T_2 \\ T_3 \\ T_4 \end{bmatrix}
$$

The thrust $T_i$ produced by each motor/propellor combination is related to the propellor's angular velocity by
$$
T_i = k\Omega_i^2.
$$

Using these expressions, we can find the actual motor commands.

Note: Given that the thrust is such that it is always overcoming gravity, a positive $m_\phi$ yields a positive motion along $\mathbf{j}^b$. A positive $m_\theta$ yields a negative motion along $\mathbf{i}^b$.


```python
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt

np.set_printoptions(formatter={'float': '{: 0.3f}'.format})

class Quadrotor(object):
    """Quadrotor
        
    This class models the physical quadrotor vehicle evolving in SE(3).
    """
    def __init__(self, r=None, Phi=None, v=None, omega=None):
        
        # internal quadrotor state
        self.r = r if r is not None else np.zeros((3,1))
        self.Phi = Phi if Phi is not None else np.zeros((3,1))
        self.v = v if v is not None else np.zeros((3,1))
        self.omega = omega if omega is not None else np.zeros((3,1))
        
        # phyiscal true parameters
        self.g = 9.81
        self.mass = 3.81
        Jxx = 0.060224; Jyy = 0.122198; Jzz = 0.132166
        self.I = np.array([[Jxx,0,0],
                           [0,Jyy,0],
                           [0,0,Jzz]])
        
        self.Mu = np.diag(np.array([0.85,0.85,0.85])) # drag
#         self.Mu = np.diag(np.array([0,0,0])) # drag
        
        # max control actuation
        self.Tmax = 40
        self.Mmax = 2
        
        # convenience
        self.Niters = 0
        
    def __str__(self):
        s  = "Quadrotor state after {} iters:\n".format(self.Niters)
        s += "\tr:     {}.T\n".format(self.r.T)
        s += "\tPhi:   {}.T\n".format(self.Phi.T)
        s += "\tv:     {}.T\n".format(self.v.T)
        s += "\tomega: {}.T\n".format(self.omega.T)
        return s
    
    @staticmethod
    def clamp(v, limit):
        v = np.copy(v)
        v[np.abs(v) > limit] = np.sign(v[np.abs(v) > limit])*limit
        return v

    @staticmethod
    def Gamma(phi, theta):
        return np.array([
            [1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)],
            [0,         np.cos(phi)      ,        -np.sin(phi)      ],
            [0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)]
        ])
        
    def Fg(self, phi, theta):
        return self.mass*self.g*np.array([[      -np.sin(theta)     ],
                                          [np.cos(theta)*np.sin(phi)],
                                          [np.cos(theta)*np.cos(phi)]])
    
    def update(self, u, dt):
        # We've been through another iteration
        self.Niters += 1
        
        # Extract Euler angles for convenience
        ph = self.Phi.flatten()[0]
        th = self.Phi.flatten()[1]
        ps = self.Phi.flatten()[2]
        
        #
        # Forces and Moments
        #
        
        u = np.array(u).flatten()
        T = np.array([[0, 0, u[0]]]).T
        M = np.array([[u[1], u[2], u[3]]]).T
        
        #
        # Saturate control effort
        #
        
        T = self.clamp(T, self.Tmax)
        M = self.clamp(M, self.Mmax)
        
        #
        # Kinematics
        #
        
        # Translational
#         f = lambda r: Rot_i_to_b(ph,th,ps).T.dot(self.v) # body velocities
        f = lambda r: self.v # inertial velocities
        self.r = rk4(f, self.r, dt)
        
        # Rotational
        f = lambda Phi: self.Gamma(Phi[0], Phi[1]).dot(self.omega)
        self.Phi = rk4(f, self.Phi, dt)
        
        #
        # Dynamics
        #
        
        # Translational
#         f = lambda v: (1/self.mass)*(self.Fg(ph,th) - T - np.cross(self.omega, v, axis=0)) # body
        f = lambda v: (1/self.mass)*(self.Fg(0,0) - Rot_i_to_b(ph,th,ps).T.dot(T) - self.Mu.dot(v)) # inertial
        self.v = rk4(f, self.v, dt)
        
        # Rotational
        f = lambda omega: np.linalg.inv(self.I).dot((-np.cross(omega, self.I.dot(omega), axis=0) + M))
        self.omega = rk4(f, self.omega, dt)
        
        # update control input
        u = np.hstack((T[2], M.flatten()))
        return u
```


```python
%%capture
%run utils.ipynb
```


```python
quad = Quadrotor()

# feed-forward thrust
z_ff = quad.mass*quad.g

Ts = 0.01
Tf = 1.5
N = int(Tf/Ts)
for i in range(N):
    m1 = 10 if Ts*i < 1 else 0
    quad.update([z_ff, 0.0, 0.0, 0.0], Ts)
print(quad)
```

    Quadrotor state after 150 iters:
    	r:     [[ 0.000  0.000  0.000]].T
    	Phi:   [[ 0.000  0.000  0.000]].T
    	v:     [[ 0.000  0.000  0.000]].T
    	omega: [[ 0.000  0.000  0.000]].T
    


## References

[1] R. W. Beard and T. W. McLain, Small Unmanned Aircraft. Princeton, New Jersey: Princeton University Press, 2012.

[2] R. W. Beard and T. W. Mclain, “Introduction to Feedback Control using Design Studies,” 2016.

[3] R. C. Leishman, J. C. MacDonald, R. W. Beard, and T. W. McLain, “Quadrotors and accelerometers: State estimation with an improved dynamic model,” IEEE Control Syst., vol. 34, no. 1, pp. 28–41, 2014.

1789bc905fced4562c249b1d0aed64ff0e5094c1
ipynb
Jupyter Notebook
quadrotor_model.ipynb
plusk01/nonlinearquad
c1bfb6a61fb739996e98fb00577d11b505f0dc71
MIT
2018-10-25T02:28:20.000Z
2022-01-18T20:33:19.000Z
quadrotor_model.ipynb
axefq/nonlinearquad
c1bfb6a61fb739996e98fb00577d11b505f0dc71
MIT
quadrotor_model.ipynb
axefq/nonlinearquad
c1bfb6a61fb739996e98fb00577d11b505f0dc71
MIT
2019-06-16T22:33:56.000Z
2021-02-12T15:20:47.000Z
Qwen/Qwen-72B
1. YES
2. YES