Advanced Hybrid Nonlinear Control for Morphing Quadrotors

The foldable quadrotor, as shown in Figure 1, consists of a rigid central body $m_o$ and four arms of masses m_2,i that is foldable around the main body using servo motors and at different angles, see Table 1 [15]. Since these angles represent the extension of the folded arm by calculating the potential control matrix for each shape, the possible shapes of this quad are (X, H, O, Y, YI, T) [14]. Servo motors of masses $m_{1, i}$ are connected to the central body and the four arms are connected to the servo motors, as well as four rotors of masses $m_{3, i}$ are located on the arms. Two rotors (2 and 4) are rotated in a clockwise direction and other two rotors (1 and 3) are rotated in a counterclockwise direction. Due to changing the quadrotor's morphology, the servo motors make the system highly coupled. The main body and servo motors are fixed around Z-axis while the arms and rotors rotate on the same axis (Z-axis). So, the most (conventional) setup leads to numerous conceivable arrangements by changing the position of the arms [15].

Some assumptions are presented to make a suitable model for the foldable quadrotor [16]:

1. The central body, servo motors, and rotation arms are rigid.
2. The propeller is assumed to be rigid, neglecting the effect of changing morphology during flight.

11.png

Figure 1. Schematic of the foldable quadrotor [15]

Table 1. Characteristics of the foldable quadrotor configuration [14]

2.1 Kinematic model of a foldable quadrotor

The mathematical model of the quadrotor is described in two frames see Figure 1. The body frame R_b(O_b, X_b, Y_b, Z_b), which is fixed and the mobile frame R_m(O_m, X_m, Y_m, Z_m). The rotation from R_b to R_m consists of three revolutions around an (X, Y, Z) axes, which means that the position is obtained at an angle (φ, ψ). So, the total rotation matrix R can be given as [15]:

$R=\left[\begin{array}{ccc}c \psi c \theta & c \psi s \theta s \varphi-s \psi c \varphi & c \psi s \theta c \varphi+s \psi s \varphi \\ s \psi c \theta & s \psi s \theta s \varphi+c \psi c \varphi & s \psi s \theta c \varphi-c \psi s \varphi \\ -s \theta & c \theta s \varphi & c \theta c \varphi\end{array}\right]$             (1)

where, $R \in S O(3)=\left\{R \in R^{3 \times 3} \mid R^T R=I_{3 \times 3, \operatorname{det}(R)=1\quad}\right\} \mathrm{s}($.$)\quad and\quad c(.)$ are abbreviations for sin(.) and cos(.), respectively. The linear velocity of the system is represented by $\mathrm{A}=\left[\begin{array}{lll}u & v & w\end{array}\right]^T \in R^3$ and angular velocity is represented by $\varsigma=\left[\begin{array}{lll}p & q & r\end{array}\right]^T \in R^3$. Both of the velocities are derived in the mobile frame R_m. The mass of the foldable quadrotor is m and the inertia is represented as $\left(\psi_i(t)\right) \in R^{3 \times 3}$.  $\xi=\left[\begin{array}{lll}X & Y & Z\end{array}\right]^T \in R^3$ describes the position and  $\eta=\left[\begin{array}{lll}\varphi & \theta & \psi\end{array}\right]^T \in R^3$ describes the orientation of the quadrotor.

2.2 Dynamic model of a foldable quadrotor

The dynamic model of the foldable quadrotor is derived from the Newton-Euler formalism and given as follows [14]:

$\left[\begin{array}{cc}m J_{3 \times 3}\left(\psi_i(t)\right) & O_{3 \times 3} \\ O_{3 \times 3} & J_{3 \times 3}\left(\psi_i(t)\right)\end{array}\right]\left[\begin{array}{c}\dot{\mathrm{A}} \\ \dot{\zeta}\end{array}\right]+\left[\begin{array}{c}\varsigma \times m \mathrm{~A} \\ \varsigma \times J\left(\psi_i(t)\right)\end{array}\right]=\left[\begin{array}{c}f \\ \tau\end{array}\right]$          (2)

where, O_3×3 is a zero-matrix dimensional 3×3, and × denotes the cross product.

Many forces act on the quadrotor, such as gravity, thrust, hub and drag. So, the total force in the mobile frame is expressed [14]:

$f=\left[\begin{array}{l}U_x \\ U_y \\ F_t\end{array}\right]+R^T\left[\begin{array}{l}F_{D x} \\ F_{D y} \\ F_{D z}\end{array}\right]+R^T\left[\begin{array}{c}0 \\ 0 \\ -m g\end{array}\right]$           (3)

The relation between the velocities and the external forces is $f=\left[f_x f_y f_z\right]^T \in R^3$ and $f_g=[00-m g]^T$ is the gravity vector, where $g$ is the gravity. $F_t$ Is The total thrust force generated by the rotation of each rotor, two components represent the hub forces $U_x, U_y$. Also, the quadrotor generates drag force $F_D \in R^3$ in the body.

The quadrotor can turn around $x_m, y_m, z_m$ directions by applying the moments $\tau_{\varphi}, \tau_\theta, \tau_\psi$. These moments depend on the folding angles of the arms and are induced by thrust forces $F_t$. Thus, the moments of roll, pitch and yaw are in the study [14].

$\tau=\left[\begin{array}{c}\tau_{\varphi}+O_x+L_x+M_{A X} \\ \tau_\theta+O_y+L_y+M_{A Y} \\ \tau_\psi+H+M_{A Z}\end{array}\right]$           (4)

Wind effects can produce an aerodynamic friction torque as [14].

$M_A=\left[M_{A x} M_{A y} M_{A z}\right]^T=\operatorname{diag}\left(k_{a x} k_{a y} k_{a z}\right) \dot{\zeta}^2$           (5)

where, $k_{a(x, y, z)}$   is the aerodynamic friction coefficient.

(O_x, O_y) Are the Gyroscopic moments, (L_x, L_y) are the flapping moment, and (H) is the hub moment. However, some phenomena have a slight effect, such as hub forces and blade flapping moments. Thus, they are neglected because the quadrotor adopted in this research does not perform any aggressive rotation of the four arms during flight [15].

2.3 Center of gravity

The possible shapes of the folding quadrotor are asymmetrical, so the center of gravity changes from one form to another, as each state has its center of gravity. As a result, it is necessary to calculate the center of gravity for each shape. The general formula of the center of gravity is shown below [6]:

$O G=\frac{\sum_{i=1}^4 \sum_{j=1}^3 \quad m_{j, i} \quad\overline{O G_{J, l}}}{m_o+\sum_{i=1}^4 \sum_{j=1}^3 \quad m_{j, i}}$           (6)

where:

$\mathrm{O}$ is the geometric center.

$\overrightarrow{O G}$ is the offset between the geometric center of the system and the global center of gravity.

$\overrightarrow{O G_o} \in R^{3 \times 1}$ is the vector of the center of gravity of the central body, which is zero.

$\overrightarrow{O G_{1, l}} \in R^{3 \times 1}$ is the vector of the center of gravity of the servo motors.

$\overrightarrow{O G_{2, l}} \in R^{3 \times 1}$ is the vector of the center of gravity of the rotating arms.

$\overrightarrow{O G_{3, l}} \in R^{3 \times 1}$ is the vector of the center of gravity of the rotors.

2.4 Control matrix

The control matrix presents the critical values of the foldable quadrotor control architecture, as it converts the square of the rotational speed of the four propellers $\left(\left.\Omega_i^2\right|_{i=1,2,3,4}\right)$ into the total thrust force $F_t$ and moments $\tau_{\varphi}, \tau_\theta, \tau_\psi\quad$ [6]:

$\Delta\left(\psi_i(t)\right)=\left[\begin{array}{cccc}b & b\left(y_{3,1}-y_g\right) & b\left(x_g-x_{3,1}\right) & d \\ b & b\left(y_{3,2}-y_g\right) & b\left(x_g-x_{3,2}\right) & -d \\ b & b\left(y_{3,3}-y_g\right) & b\left(x_g-x_{3,3}\right) & d \\ b & b\left(y_{3,4}-y_g\right) & b\left(x_g-x_{3,4}\right) & -d\end{array}\right]^T$               (7)

Define the control vector of its altitude and attitude as [6]: $u=\left[\begin{array}{llll}u_1 & u_2 & u_3 & u_4\end{array}\right]^T \in R^{4 \times 4}$, where,

$\left[\begin{array}{l}u_1 \\ u_2 \\ u_3 \\ u_4\end{array}\right]=\Delta\left(\psi_i(t)\right)\left[\begin{array}{l}\Omega_1^2 \\ \Omega_2^2 \\ \Omega_3^2 \\ \Omega_4^2\end{array}\right]$               (8)

b and d are the aerodynamic coefficients and are taken as [6]:

$f_i=b \Omega_i^2, Q_i=d \Omega_i^2$               (9)

where, b is the Thrust coefficient and d is the drag coefficient.

The foldable quadrotor has eight control inputs. The primary four inputs deliver the control vector altitude and attitude. The second four control inputs provide the servo motors' control vector. To establish the complete dynamic model, a state space vector is given, which consists of a set of inputs, outputs and states associated with first-order differential equations, which will form a model of the space. A space with state variables as its axes is referred to as a "state space”. The state of the system can be represented as a vector. The general state space model is represented as following:

$\left\{\begin{array}{c}\dot{x}(t)=A x(t)+B u(t) \\ y(t)=c x(t)\end{array}\right.$           (10)

where, x(t), y(t), and u(t) presented the state, output and control vector, respectively. A, B, and C shows system matrix, input and output matrix, respectively.

So, based on Eqs. (2), (3) and (4), the state space vector of the nonlinear dynamic model is given as [14]:

$\mathrm{X}=\lceil\mathrm{x} \dot{\mathrm{x}} \mathrm{y} \dot{\mathrm{y}} \mathrm{z} \dot{\mathrm{z}} \varphi \dot{\varphi} \theta \dot{\theta} \psi \dot{\psi}\rceil^T$           (11)

Also, the state space vector is written as [14]:

$X=\left[\begin{array}{lllllllllllll}x_1 & x_2 & x_3 & x_4 & x_5 & x_6 & x_7 & x_8 & x_9 & x_{10} & x_{11} & x_{12}\end{array}\right]^T$           (12)

$\left\{\begin{array}{c}\dot{x}_1=x_2 \\ \dot{x}_2=U_1 \frac{u_x}{m}+b_1 x_2 \\ \dot{x}_3=x_4 \\ \dot{x}_4=U_1 \frac{u_y}{m}+b_2 x_4 \\ \dot{x}_5=x_6 \\ \dot{x}_6=-g+U_1 \frac{\cos \varphi \cos \theta}{m}+b_3 x_6 \\ \dot{x}_7=x_8 \\ \dot{x}_8=a_1 \dot{\theta} \dot{\psi}+a_4 \dot{\theta} \Omega_r+\frac{1}{J_{x x}} U_2+b_4 \dot{\varphi}^2 \\ \dot{x}_9=x_{10} \\ \dot{x}_{10}=a_2 \dot{\varphi} \dot{\psi}+a_5 \dot{\varphi} \Omega_r+\frac{1}{J_{y y}} U_3+b_5 \dot{\theta}^2 \\ \dot{x}_{11}=x_{12} \\ \dot{x}_{12}=a_3 \dot{\varphi} \dot{\theta}+\frac{1}{J_{z z}} U_4+b_6 \dot{\psi}^2\end{array}\right.$           (13)

where, $a_1=\frac{J_{y y}-J_{z z}}{J_{x x}}, a_2=\frac{J_{z z}-J_{x x}}{J_{y y}}, a_3=\frac{J_{x x}-J_{y y}}{J_{z z}}, a_4=\frac{-J_r}{J_{x x}}$, $a_5=\frac{J_r}{J_{y y}}, \quad b_1=\frac{-k_{D x}}{m}, b_2=\frac{-k_{D y}}{m}, b_3=\frac{-k_{D z}}{m}, b_4=\frac{-k_{A x}}{J_{x x}}$, $b_5=\frac{-k_{A y}}{J_{y y}}, b_6=\frac{-k_{A z}}{J_{z z}}$

$u_x=\sin \Psi \sin \varphi+\cos \Psi \sin \theta \cos \varphi$           (14)

$u_y=\sin \Psi \sin \theta \cos \varphi-\cos \Psi \sin \varphi$           (15)

$\Omega_r=\Omega_1^2-\Omega_2^2+\Omega_3^2-\Omega_4^2$           (16)

The design process for the inner loop (u_1, u_2, u₃ u₄) is based on the sliding mode control method, while the design for control signals (u_x, u_y) for the motion in the (x, y) plane are based on the nonlinear PID (NLPID) control, see Figure 2.

2.png

Figure 2. Proposed foldable quadrotor control design

4.1 Design of inner loop Sliding Mode Controller (SMC)

For the altitude (Z) [14]:

$\left\{\begin{array}{c}\dot{x}_5=x_6 \\ \dot{x}_6=-g+U_1 \frac{\cos \varphi \cos \theta}{m}+b_3 x_6\end{array}\right.$         (23)

The tracking error of the altitude is e₅ is expressed as:

$e_5=x_5-x_{5 d}$         (24)

$\dot{e}_5=\dot{x}_5-\dot{x}_{5 d}=x_6-\dot{x}_{5 d}$         (25)

$\ddot{e}_5=\ddot{x}_5-\ddot{x}_{5 d}=\dot{x}_6-\ddot{x}_{5 d}$         (26)

where, x₅ is the actual altitude and x_5d is the desired altitude.

The sliding surface for the altitude is:

$s_5=\dot{e}_5+c_5 e_5$              (27)

Taking the derivative of the sliding surface:

$\dot{s}_5=\ddot{e}_5+c_5 \dot{e}_5$              (28)

To guarantee $\dot{s}_5=0$, U₁ must be:

$U_1=\frac{m}{\cos x_7 \cos x_9}\left[\ddot{x}_{5 d}+g-b_3 x_6-c_5\left(x_6-\dot{x}_{5 d}\right)-k_5 \operatorname{sgn}\left(s_5\right)-k_6 s_5\right]$              (29)

Repeat the same steps as above to find the control signal for the attitude (φ, θ and ψ), so:

$U_2=J_{x x}\left[\ddot{x}_{7 d}-a_1 x_{10} x_{12}-a_4 x_{10} \Omega_r-b_4 x_8^2-c_7\left(x_8-\dot{x}_{7 d}\right)-k_7 \operatorname{sgn}\left(s_7\right)-k_8 s_7\right]$             (30)

$U_3=J_{y y}\left[\ddot{x}_{9 d}-a_2 x_8 x_{12}-a_4 x_8 \Omega_r-b_5 x_9^2-c_9\left(x_{10}-\dot{x}_{9 d}\right)-k_9 \operatorname{sgn}\left(s_9\right)-k_{10} s_9\right]$              (31)

$U_4=J_{z z}\left[\ddot{x}_{11 d}-a_3 x_8 x_{10}-b_6 x_{12}^2-c_{11}\left(x_{12}-\dot{x}_{11 d}\right)-k_{11} \operatorname{sgn}\left(s_{11}\right)-k_{12} s_{11}\right]$              (32)

4.2 Design of outer loop Nonlinear PID Controller (NLPID)

The Quadrotor system has no actual control input for the motion in the (X, Y) plane. The following analysis is proposed to generate suitable control signals (u_x, u_y) for the motion in the (X, Y) plane:

From Eqs. (14) and (15) [14]:

$u_x=\sin \psi_d \sin \varphi_d+\cos \psi_d \sin \theta_d \cos \varphi_d$

$u_y=\sin \psi_d \sin \theta_d \cos \varphi_d-\cos \psi_d \sin \varphi_d$

By solving these two equations:

$\varphi_d=\operatorname{asin}\left(u_x \sin \psi_d-u_y \cos \psi_d\right)$              (33)

$\theta_d=\operatorname{asin}\left(\frac{u_x \cos \psi_d+u_y \sin \psi_d}{\cos \varphi_d}\right)$              (34)

The above two equations represent the desired value for roll and pitch angles. The main objective of the NLPID controller is to approach the errors of (x and y) positions to zero as described below:

$\ddot{e}_1+U_{N L P I D}=0$              (35)

$\ddot{x}_1-\ddot{x}_{1 d}+f_{x 1}\left(e_x\right)+f_{x 2}\left(\dot{e}_x\right)+f_{x 3}\left(\int e_x d t\right)=0$              (36)

$\ddot{e}_3+U_{N L P I D}=0$              (37)

$\ddot{x}_3-\ddot{x}_{3 d}+f_{y 1}\left(e_y\right)+f_{y 2}\left(\dot{e}_y\right)+f_{y 3}\left(\int e_y d t\right)=0$              (38)

Letting (u_x, u_y) be the virtual control signals for (x, y) respectively, thus:

$u_x=\frac{m}{U_1}\left[\frac{k_{d x}}{m} x_2+\ddot{x}_{1 d}-f_{x 1}\left(e_x\right)-f_{x 2}\left(\dot{e}_x\right)+f_{x 3}\left(\int e_x d t\right)\right]$              (39)

$u_y=\frac{m}{U_1}\left[\frac{k_{d y}}{m} x_4+\ddot{x}_{3 d}-f_{y 1}\left(e_y\right)-f_{y 2}\left(\dot{e}_y\right)+f_{y 3}\left(\int e_y d t\right)\right]$              (40)

The nonlinear system of the 6-degrees of freedom and the proposed controller for this quadrotor is simulated using Matlab/Simulink. This foldable quadrotor is simulated under ideal conditions (no external disturbances or uncertainties affected). The parameter values used in the simulation are listed in Table 2 [15]. Since this quadrotor has the ability to change the configuration according to the tasks assigned to it, it will have an important effect on the inertial matrix since each shape has a specific value of inertia, as in the Table 3 [15].

Table 2. Foldable quadrotor parameters [15]

Table 3. The possible inertia values for each shape [15]

The Fireflies algorithm is used to find the lowest value of the proposed cost function (Eq. (45)). The parameters used in the firefly algorithm are shown in Table 4. A reference path, shown in Figure 3, is applied. To calculate the cost function to set 32 parameters of the model, 24 parameters are used for the nonlinear PID controller, see Table 5, and 8 parameters are used for the sliding mode control, see Table 6. Satisfactory results were reached by proving that the minimum cost function reached as shown in Figure 4.

Followed the desired trajectory as when $0<t<20 \mathrm{sec}$., $x_d=0 ; 20<t<30, x_d=t-20 ; 30<t<50, x_d=10$; $50<t<60, x_d=-1(t-50) ; 60<t<70, x_d=0$. When $0<t<30, y_d=0 ; 30<t<40, y_d=t-30 ; 40<$ $t<60, y_d=10 ; 60<t<70, y_d=-1(t-60)$. and when $0<t<70, z_d=10$. The quadrotor flights upward with a fixed path until it reaches a height of $10 \mathrm{~m}$ from the initial positions $(X, Y, Z)=(0,0,0)$.

The simulation results were carried out using MATLAB/Simulink with a time set to 70 sec, As shown in the Figures 5 and 6 below.

Table 4. Firefly algorithm parameters

Table 5. Optimal parameters for nonlinear PID

Table 6. Optimal parameters for sliding mode controller

3.png

Figure 3. Performance of RMSE using Firefly algorithm

4.png

Figure 4. Control of a foldable quadrotor using trapezoidal trajectory

Figures 5 and 6 show the proposed controller's efficiency in ensuring the stability of all possible shapes of the quadrotor while following the proposed path as the error in position approaches zero. Figure 7 shows that the error in the x-axis position varies between 0.02 m to -0.02 m and approaches zero. The error in the y-axis position fluctuates between 0.024 m to -0.024 m and settles at 0.0006686 m.

5a.png

(a) X trajectory

5b.png

(b) Y trajectory

5c.png

(c) Z trajectory

5d.png

(d) Yaw angle

Figure 5. Response of the position and yaw angle: (a) X trajectory; (b) Y trajectory; (c) Z trajectory; (d) Yaw angle

6a.png

(a) Tracking error in X position

6b.png

(b) Tracking error in Y position

6c.png

(c) Tracking error in Z position

6d.png

(d) Tracking error in yaw angle

Figure 6. Tracking error in position and yaw angle: (a) Tracking error in X position; (b) Tracking error in Y position; (c) Tracking error in Z position; (d) Tracking error in yaw angle

The square shape path, shown in Figure 5, is applied and the quadrotor flights upward with a fixed path until it reaches a height of 5 m from the initial positions (0,0,0). It is noticed from Figure 8 that the quadrotor follows the required path angles with small errors as changed the folded angles every 12 seconds. Using a switch, the foldable angles toggle between these values allowing the quadrotor to change its morphology.

7a.png

(a) first arm angle ψ₁

7b.png

(b) second arm angle ψ₂

7c.png

(c) third arm angle ψ₃

7d.png

(d) fourth arm angle ψ₄

Figure 7. Servomotor's angle variations: (a) first arm angle ψ₁; (b) second arm angle ψ₂; (c) third arm angle ψ₃; (d) fourth arm angle ψ₄

8a.png

(a) Evolution of the command U₁

8b.png

(b) Evolution of the command U₂

8c.png

(c) Evolution of the command U₃

8d.png

(d) Evolution of the command U₄

Figure 8. Control signals. (a) Evolution of the command U₁; (b) Evolution of the command U₂; (c) Evolution of the command U₃; (d) Evolution of the command U₄

The value of the control signal U₁ changes during the trajectory to reach its highest value after 40 seconds and settles at a fixed value of around 17.76 N. As for the second and third control signals U₂, U₃, differences appear due to the presence of the multirotor. The fourth control signal U₄ takes the highest value at the beginning of the path with the presence of fluctuations and then stabilizes at zero.