Keywords

1 Introduction

The Parrot Mambo minidrone is an example of a new way of bringing the teaching of STEM (science, technology, engineering, mathematics) subjects into the classroom [1]. It offers a variety of tools and approaches that make it suitable for students from primary school to PhD level. In fact, it supports the best coding platforms, providing the end user with plenty of choices for tackling heterogeneous STEM problems. In particular, for primary schools it offers block coding with platforms like Tynker and Blocky; for secondary schools and universities it offers text coding with JavaScript and Python and also compatibility with MATLAB & Simulink [2]. Simulink is the environment used to implement a robust nonlinear control algorithm (sliding mode control), in order to demonstrate the flexibility and the versatility of this novel educational ecosystem: from block coding to state-of-the-art prototyping, implementation and testing of experimental control approaches. This paper considers a mathematical model of the Parrot Mambo, which was used to modify the classical sliding mode control algorithm in order to get enhanced robustness against disturbances.

2 Quadrotor Mathematical Model

As shown in Fig. 1, the quadrotor contains four motors placed in a cross-configuration (front and rear propeller rotate counterclockwise, while the right and the left ones rotate clockwise) [3]. Each rotor is linked to a propeller. The quadrotor is an underactuated system: in fact, it has 6 DOF (degree of freedom), three Cartesian coordinates (X, Y, Z) and three attitude angles, ϕ, θ and ψ (roll, pitch and yaw, respectively) but only four rotors/motors. However, it is possible to choose four controls to set the altitude (vertical distance from the ground) and the attitude (angular position) of the drone, that is, Throttle Uz: vertical force obtained by increasing or decreasing the speed of the four propellers by the same amount; Roll Uϕ: torque with respect to the x axis of the body frame, obtained by increasing or decreasing the speed of either the left or the right propellers; Pitch Uθ: torque with respect to the y axis of the body frame, obtained by increasing or decreasing the speed of either the front or the rear propellers; Yaw Uψ: torque with respect to the z axis of the body frame, obtained by increasing (or decreasing) the speed of both the front and the rear propellers, while decreasing (or increasing) both the left and the right propellers. Two coordinate frames are used to describe the motion of the drone: the earth-centered inertial frame and the body-fixed frame. The earth frame \(\left( {\begin{array}{*{20}l} {O_{e} } \hfill & {x_{e} } \hfill & {y_{e} } \hfill & {z_{e} } \hfill \\ \end{array} } \right)\) is used to define the position vector of the quadrotor \(\xi = \left[ {\begin{array}{*{20}l} X \hfill & Y \hfill & Z \hfill \\ \end{array} } \right]^{T}\) expressed in meters and the vector of Euler angles \(\sigma = \left[ {\begin{array}{*{20}l} \phi \hfill & \theta \hfill & \psi \hfill \\ \end{array} } \right]^{T}\) expressed in radians. The body frame \(\left( {\begin{array}{*{20}l} {O_{{b{\text{}}}} } \hfill & {x_{{b{\text{}}}} } \hfill & {y_{{b{\text{}}}} } \hfill & {z_{b}} \hfill \\ \end{array} } \right)\) defines the vector of linear velocities \(\mu = \left[ {u~v~w} \right]^{T}\) expressed in meters per seconds, the vector of angular velocities \(\omega = \left[ {\begin{array}{*{20}c} P & Q & R \\ \end{array} } \right]^{T}\) expressed in radians per seconds, the forces F expressed in newton and the torques \(\tau\) expressed in newton meters. The dynamic model was derived using the Newton–Euler formalism. As seen in [4], the dynamics of the 6 DOF rigid body under the effect of external forces and torques are:

$$\left[ {\begin{array}{*{20}l} {mI_{{3,3}} } \hfill & 0 \hfill \\ 0 \hfill & J \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\dot{\mu }} \\ {\dot{\omega }} \\ \end{array} } \right] + \left[ {\begin{array}{*{20}c} {\omega \times m\omega } \\ {\omega \times J\omega } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} F \\ \tau \\ \end{array} } \right]$$
(1)
Fig. 1
figure 1

Quadrotor structure representation

Full size image

In order to develop Eq. (1), the different forces and torques that affect the quadrotor need to be determined. The main forces that affect the system are:

  • Input forces and torques: directly produced by the four rotors. As a result of rotation, each motor exhibits an aerodynamic moment \(M_{i}\) and a lift force \(F_{i}\) given by [5]:

    $$F_{i} = C_{T} \Omega _{i}^{2} ,\;M_{i} = C_{q} \Omega _{i}^{2}$$
    (2)

    where \(C_{T}\) and \(C_{q}\) are the thrust and the torque coefficient respectively, \(\Omega _{i}\) is the velocity of the \(i^{{th}}\) rotor. As reported in [6], the resultant input torque vector is:

    $$\tau _{{in}} = \left[ {\begin{array}{*{20}c} {U_{\phi } } \\ {U_{\theta } } \\ {U_{\psi } } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}l} {C_{T} d\left( {\Omega _{4}^{2} - \Omega _{2}^{2} } \right)} \hfill \\ {C_{T} d\left( {\Omega _{3}^{2} - \Omega _{1}^{2} } \right)} \hfill \\ {C_{q} \left( { - \Omega _{1}^{2} + \Omega _{2}^{2} - \Omega _{3}^{2} + \Omega _{4}^{2} } \right)} \hfill \\ \end{array} } \right].$$
    (3)

    The vector of input forces with respect to the body frame is:

    $$F_{{in}}^{B} = \left[ {\begin{array}{*{20}l} 0 \hfill \\ 0 \hfill \\ {U_{Z} } \hfill \\ \end{array} } \right] = \left[ {\begin{array}{*{20}l} 0 \hfill \\ 0 \hfill \\ {C_{T} \left( {\Omega _{1}^{2} + \Omega _{2}^{2} + \Omega _{3}^{2} + \Omega _{4}^{2} } \right)} \hfill \\ \end{array} } \right].$$
    (4)

  • Gyroscopic effect: this is produced by rotation of the propeller and it is given as:

    $$\tau _{g} = \left[ {\begin{array}{*{20}c} { - \mathop \sum \limits_{{i = 1}}^{4} J_{{tp}} \left[ {\begin{array}{*{20}c} {\omega \times \left( {\begin{array}{*{20}c} 0 \\ 0 \\ 1 \\ \end{array} } \right)} \\ \end{array} } \right] - ( - 1)^{i} \Omega _{i} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}l} { - Q\Omega J_{{tp}} } \hfill \\ {P\Omega J_{{tp}} } \hfill \\ 0 \hfill \\ \end{array} } \right]$$
    (5)

    where \(J_{{tp}}\) is the total rotational moment of inertia around the propeller axis and \(\Omega\) is the sum \(\left( {\Omega = - \Omega _{1} + \Omega _{2} - \Omega _{3} + \Omega _{4} } \right)\) of the four propeller speeds expressed in radians per seconds.

  • Gravitational force:

    $$F_{G} = \left[ {\begin{array}{*{20}l} 0 \hfill \\ 0 \hfill \\ { - mg} \hfill \\ \end{array} } \right]$$
    (6)

    where g is the gravitational acceleration and m the mass of the drone.

Replacing Eqs. (2)–(6) into Eq. (1), and expressing the rotational subsystem with respect to the earth frame, the dynamics equations become [6]:

$$\left\{ {\begin{array}{*{20}l} {\ddot{X} = \left( {S_{\phi } S_{\psi } + C_{\psi } S_{\theta } C_{\phi } } \right)\frac{{U_{z} }}{m}} \hfill \\ {\ddot{Y} = - \left( {S_{\phi } C_{\psi } + S_{\psi } S_{\theta } C_{\phi } } \right)\frac{{U_{z} }}{m}} \hfill \\ {\ddot{Z} = - g + C_{\theta } C_{\phi } \frac{{U_{z} }}{m}} \hfill \\ {\ddot{\phi } = \frac{1}{{J_{{xx}} }}\left( {U_{\phi } + \left( {J_{{yy}} - J_{{zz}} } \right)\dot{\theta }\dot{\psi } + \dot{\theta }\Omega J_{{tp}} } \right)} \hfill \\ {\ddot{\theta } = \frac{1}{{J_{{yy}} }}\left( {U_{\theta } + \left( {J_{{zz}} - J_{{xx}} } \right)\dot{\phi }\dot{\psi } - \dot{\phi }\Omega J_{{tp}} } \right)} \hfill \\ {\ddot{\psi } = \frac{1}{{J_{{zz}} }}\left( {U_{\psi } + \left( {J_{{xx}} - J_{{yy}} } \right)\dot{\phi }\dot{\theta }} \right)} \hfill \\ \end{array} } \right.$$
(7)

3 Sliding Mode Controller

The sliding mode control technique consists of a high-speed switching control law that forces the state trajectories to follow a manifold called switching surface, and keeps the trajectories on this surface [7]. Sliding mode control design is a two-step procedure: sliding surface synthesis to provide the desired performance, and control design to ensure that all motions slide along the desired surfaces. The sliding mode control (SMC) is composed of equivalent control and corrective control [8]. The former ensures that the trajectories stay on the surface. The latter ensures that the trajectories reach the surface and compensates for any variations around the surface, whether due to external disturbances or to unmodeled dynamics. The sliding control for a given state is:

$$U_{i} = U_{i}^{{eq}} + U_{i}^{C}$$
(8)

where \(U_{i}^{{eq}}\) is the equivalent control and \(U_{i}^{C}\) is the corrective control. As in [9], let the sliding surface be of the form:

$$S_{i} = \mathop {e_{i} }\limits^{.} + \alpha _{i} e_{i}$$
(9)

where \(S_{i}\) is the sliding surface for a given state, \(e_{i}\) is the error between the actual state and the desired value, and \(\alpha _{i}\) is a positive proportional gain. As for most first order systems, the chosen Lyapunov function is:

$$V_{i} = \frac{1}{2}S_{i}^{2} .$$
(10)

To ensure system stability, the time derivative of the Lyapunov function \(\left( {\mathop {V_{i} }\limits^{.} = S_{i} \mathop {S_{i} }\limits^{.} } \right)\) must strictly be negative. To ensure that, we can set [6]:

$$\mathop {S_{i} }\limits^{.} = - \beta _{i} S_{i} - \lambda _{i} sign\left( {S_{i} } \right).$$
(11)

by the corrective control. As is well known, sliding mode control is robust against matched bounded disturbances. However, as proposed in [6], a more realistic assumption on unmodeled dynamics and external disturbances can be represented as:

$$d_{i} = \rho _{i} \left| {S_{i} |^{{p_{i} }} } \right|U_{i} |^{{v_{i} }} + d_{{0i}}$$
(12)

where 0 < vi < 1, 0 ≤ ρi ≤ 1, pi is a positive real number and d0i is a bounded disturbance. With corrective control the surface dynamics take the form [6]:

$$\mathop {S_{i} }\limits^{.} = \left( { - \beta _{i} |S_{i} |^{{m_{i} }} - \lambda _{i} } \right)sign\left( {S_{i} } \right) + d_{i}$$
(13)

where mi is a positive integer. To respect the condition of a strictly negative derivative of Eq. (10) we can take [6]:

$$m_{i} = \frac{{p_{i} + 1 - v_{i} }}{{1 - v_{i} }}.$$
(14)

and the control algorithm is [6]:

$$\left\{ \begin{aligned} U_{z} & = \frac{m}{{\cos \theta \cos \phi }}\left( {\mathop {Z^{d} }\limits^{{..}} + g + \alpha _{z} \mathop {e_{z} }\limits^{{..}} + \beta _{z} \left| {S_{z} } \right|^{{\frac{{p_{z} + 1 - v_{z} }}{{1 - v_{z} }}}} sign\left( {S_{z} } \right) + \lambda _{z} sign\left( {S_{z} } \right)} \right) \\ U_{\phi } & = J_{{xx}} \left( {\mathop {\phi ^{d} }\limits^{{..}} + \beta _{\phi } |S_{\phi } |^{{\frac{{p_{\phi } + 1 - v_{\phi } }}{{1 - v_{\phi } }}}} sign\left( {S_{\phi } } \right) + \lambda _{\phi } sign\left( {S_{\phi } } \right) + \alpha _{\phi } \mathop {\dot{e}_{\phi } } } \right) \\ & \quad - \left( {J_{{yy}} - J_{{zz}} } \right)\mathop {\dot{\theta} \psi } - \dot{\theta }\Omega J_{{tp}} \\ U_{\theta } & = J_{{yy}} \left( {\mathop {\theta ^{d} }\limits^{{..}} + \beta _{\theta } |S_{\theta } |^{{\frac{{p_{\theta } + 1 - v_{\theta } }}{{1 - v_{\theta } }}}} sign\left( {S_{\theta } } \right) + \lambda _{\theta } sign\left( {S_{\theta } } \right) + \alpha _{\theta } \mathop {\dot{e}_{\theta } } } \right) \\ & \quad - \left( {J_{{zz}} - J_{{xx}} } \right)\mathop {\dot{\phi} \dot{\psi }} + \dot{\phi }\Omega J_{{tp}} \\ U_{\psi } & = J_{{zz}} \left( {\mathop {\psi ^{d} }\limits^{{..}} + \beta _{\psi } |S_{\psi } |^{{\frac{{p_{\psi } + 1 - v_{\psi } }}{{1 - v_{\psi } }}}} sign\left( {S_{\psi } } \right) + \lambda _{\psi } sign\left( {S_{\psi } } \right) + \alpha _{\psi } \mathop {\dot{e}_{\psi } } } \right) \\ & \quad - \left( {J_{{xx}} - J_{{yy}} } \right)\mathop {\dot{\phi} \dot{\theta }} \\ \end{aligned} \right.$$
(15)

4 Experimental Results

The performance of the control algorithm deployed on the Parrot Mambo drone are shown below. The reference trajectory is shown in red (dashed line) and the actual trajectory is shown in blue (continuous line). In Fig. 2, the weight of the drone has been modified (its weight has been increased by 16% compared with the nominal mass) and we can see that the control algorithm, with surface dynamics of Eq. (11), cannot compensate for this weight increase, since there is an offset of about 10 cm in altitude (see Fig. 2a). On the other hand, the control algorithm with surface dynamics of Eq. (13), after a transient, is able to reach the desired altitude (see Fig. 2b). In Fig. 3, the yaw angle is shown, and the drone is in an environment with disturbances that are created artificially (wind fan). In Fig. 3a, spikes and an oscillatory behavior are seen for the control algorithm with surface dynamics of Eq. (11). In Fig. 3b, the control algorithm, with surface dynamics of Eq. (13), eliminates the spikes, and the behavior of the drone does not differ more than 0.1 radians with respect to the reference signal.

Fig. 2
figure 2

Altitude control

Full size image
Fig. 3
figure 3

Yaw control

Full size image

5 Conclusions

This paper has proposed a sliding mode control to control the altitude and the attitude of the quadrotor. The derived quadrotor model and the controller have been tested on a real drone, and the results demonstrate the robustness and the effectiveness of the control algorithm. What has been shown so far also demonstrates the flexibility and versatility of this new educational ecosystem: from block programming (via Simulink) to prototyping, to implement and test cutting-edge experimental and research control approaches.