• date_range 14/03/2020 info
sort label
Robotics
##### Introduction

Quadrotors are rotorcrafts whose movements are driven by four motors. Particularly, two pairs of propellers rotated opposite to each other are used to generate thrusts, and hence, lift the quadrotor in the air. When neglecting aerodynamics effects, the motion of a quadrotor is controlled by the gravity and four individual thrusts. We consider the quadrotor as a rigid body whose motion can be divided into the translational motion of the center of the gravity and the rotational motion around the center of the gravity.

Translational Motion and Force

We can describe the translational motion of a rigidy body in a 3D Cartesian coordinate system using the position vector $\mathbf{p}=[x, y, z]$ and its derivatives, including the velocity $\mathbf{v}$, the acceleration $\mathbf{a}$, the jerk $\mathbf{j}$, and the snap $\mathbf{s}$.

Force is a translational quantity which causes a rigid body to move. The translational movement of a quadrotor is driven by three different forces: gravity, total thrust {force generated by propellers}, and drag force {due to linear friction}.

\begin{aligned} f_g &= mg & ~ \text{Gravity}\\ f_i &= k_2^f \omega^2 + k_1^f \omega + k_0^f & ~ \text{Single thrust of each propellers} \\ f_d &= -k_d v & ~ \text{Drag Force} \end{aligned}

where $g=9.81$ is the gravity acceleration, $m$ is the mass of the quadrotor, $\omega$ is the rotational speed of the propelles and $k_i, i\in [0, 1,2]$ are polynominal coefficients. Here, $v$ is the linear velocity of the quadrotor and $k_d$ a coefficient. Hence, the translational motion of a quadrotor can be derived from the following differential equations:

\begin{aligned} \mathbf{ \dot{p} } &= \mathbf{v} \\ \mathbf{\dot{v}} &= \begin{bmatrix} 0 \\ 0 \\ -g \end{bmatrix} + \mathbf{R}_{WB} \begin{bmatrix} 0 \\ 0 \\ \sum_i^4 (k_2^f \omega_i^2 + k_1^f \omega_i + k_0^f) / m \end{bmatrix} + \begin{bmatrix} -k_d v_x \\ -k_d v_y \\ -k_d v_z \\ \end{bmatrix}/m \end{aligned}

where $\mathbf{R}_{WB}$ is a rotation matrix from the body fram $B$ to the world frame $W$.

Rotational Motion and Torque

The orientation of a rigidy body has several different representations, such as Euler angles, axis-angle representations, rotation matrix, or quaternions. Euler angles,

$[\theta_x, \theta_y, \theta_z] \rightarrow \text{roll}, \text{pitch}, \text{yaw}$

are simple and intuitive to understand, however, it suffers from the problem of “gimbal lock,” which prevents them from measuring orientation when the pitch angle approaches +/- 90 degrees. On the other hand, quaternions can be more complex compared to euler angles, but, are alternative representations that do not suffer from gimbal lock. We denote

$\mathbf{q}_\text{WB} = [q_w, q_x, q_y, q_z]$

as the quaternion that describes the orientation of body frame $B$ with respect to the world frame $W$. To rotate a vector $\mathbf{c}$ from its body frame $B$ to the world frame $W$ by the quaternion $$\mathbf{q}_{WB}$$, we use the notion $$\mathbf{c}_W=\mathbf{q}_{WB} \odot \mathbf{c}_B$$.

##### Dynamics

The dynamical model of a quadrotor is denoted via a set of Ordinary Differential Equations. We use the state vector $\mathbf{s}=[\mathbf{p}, \mathbf{q}, \mathbf{v}]$.

\begin{aligned} \mathbf{ \dot{p} } &= \mathbf{v} \\ \mathbf{ \dot{v} } &= \mathbf{q} \odot \mathbf{c} - \mathbf{g}\\ \mathbf{ \dot{q} } &= \frac{1}{2} \mathbf{\Lambda} ( \boldsymbol{\omega}) \cdot \mathbf{q} \\ \end{aligned}
def f(x, u):
"""
System Dynamics: dx = f(x, u)
"""
thrust, wx, wy, wz = action
qw, qx, qy, qz = x[kQuatW:kQuatZ+1]
#
dx = np.zeros(shape=s_dim)

# derivative of position
dx[kPosX:kPosZ+1] = x[kVelX:kVelZ+1]
# derivative of quaternion
dx[kQuatW] = 0.5 * ( -wx*qx - wy*qy - wz*qz )
dx[kQuatX] = 0.5 * (  wx*qw + wz*qy - wy*qz )
dx[kQuatY] = 0.5 * (  wy*qw - wz*qx + wx*qz )
dx[kQuatZ] = 0.5 * (  wz*qw + wy*qx - wx*qy )
# derivative of velocity
dx[kVelX] = 2 * ( qw*qy + qx*qz ) * thrust
dx[kVelY] = 2 * ( qy*qz - qw*qx ) * thrust
dx[kVelZ] = (qw*qw - qx*qx -qy*qy + qz*qz) * thrust - gz
return dx