Nonlinear Quadrotor Control Based on Simulink Support Package for Parrot Minidrones ? ??

Abstract

This paper deals with nonlinear control design for Parrot Mambo or Parrot Rolling Spider quadcopters using the Simulink Support Package for Parrot Minidrones. A full rigid body model of the ying vehicle that doesn't assume smallness of the Euler angles is considered. For synthesis of the control the nonlinear dynamics inversion and integrator backstepping approaches are used. Block diagrams illustrate how the control laws are applied to Parrot Minidrone ight control and can be used in nonlinear control education. Exercises to design nonlinear Parrot Minidrone control algorithms as Simulink Subsystem blocks are suggested.

Keywords

• Nonlinear control Control education Integrator backstepping

-

During the last two decades control of quadrotors became extremely popular among control theorists and practitioners. There are many reasons for such popularity. In spite of the fact that a quadrotor is inherently an underactuated mechanical system, it demonstrates nice controllability properties. This turned quadrotors into a test application for many control theories. Moreover, in contrast to wide-spread in control theory academical or educational mechanical examples, see e.g. [4], quadcopters can be considered as real industrial systems which are employed in many civil and military tasks.

Such theoretical and practical appeal resulted in a bulk of papers, see e.g. [2, 3, 5{7, 9{12, 15{17]. To solve position reference trajectory tracking control problems di erent approaches can be found in the literature, e.g. the PID and LQR control (see [7, 10]), integrator backstepping based designs ([3, 11]) or neural networks (see e.g. [2]). Unknown model parameters, in particular, uknown quadcopter mass and moments of inertia, were accounted for in [1] and [13]. Still, it remains a challenge for a quadcopter control system to account for the in uence of external uncontrolled disturbances, e.g. wind, and to satisfy state and control constraints during quadrotor motion, see e.g. [14{16].

The main feature of the current paper is that the suggested nonlinear control algorithms and the corresponding block diagrams are presented for the purpose of implementation on the Parrot Minidrones (Parrot Mambo or Parrot Rolling Spider) using the Simulink Support Package for Parrot Minidrones (SSPPM) [18]. This package is included in the Matlab environment and is being actively developed. SSPPM allows to create Parrot Minidrones ight control algorithms using Simulink blocks and deploy control algorithms directly on the drone via a Bluetooth wireless network.

Parrot Minidrones together with the SSPPM can be considered as a nice and a ordable control laboratory equipment due to a very low price of the minidrones and availability of the Matlab/Simulink environment at technical universities. The Parrot Mambo and Parrot Rolling Spider Minidrones are equipped with an ultrasonic sensor, accelerometer, gyroscope, pressure sensor and a downward facing camera, from which one can restore acceleration, angular velocity, altitude and displacement in the horizontal plane.

The appeal of using Parrot Minidrones together with the SSPPM for control education purposes is also underpinned by the fact that the SSPPM is a thorough modelling environment ready for implementation on the hardware. It contains all necessary default control system components, such as a quadrotor nonlinear mathematical model with the identi ed physical parameters, the state estimator subsystem that recovers state of the model from the measured data, a tuned PID controller to realize some basic angular and position reference motions. Moreover, researchers can replace any component of the system with their own one and test how their control or state estimation algorithm performs on a trueto-life quadrotor model or real ying device.

The paper is organized as follows. The quadcopter equations of motion are revised in Section 2. The synthesis of nonlinear control for tracking reference altitude and angular position trajectories is considered in Section 3. Section 4 presents design of nonlinear control for tracking reference position trajectories. Nonlinear adaptive control in case when the quadcopter mass and its moments of inertia are treated as unknown constants is discussed in Section 5. Section 6 gives numerical simulation and experimental results. Finally, the paper concludes with some remarks in Section 7. 2

Mathematical Model of Quadcopter Motion Consider a quadcopter rigid body model, with translational and rotational dynamics described by the following systems, respectively, (see e.g. [5, 10]): m  = F cos cos sin cos sin sin

Let us note that for a quadcopter the thrust F and the vector of torques M are functions of the four rotor angular velocities i and can be modeled by 0 1 sin tg C = @ 0 cos 0 sin sec cos tg 1 sin cos sec

A :

l b=k 1 1 l 0 0 l b=k b=k 1 where fi = k i2 is the thrust force of the i-th rotor; k and b are the rotors lift and drag aerodynamic coe cients, respectively; l is the distance between the quadrotor center of mass and the rotors. 3

Attitude and Altitude Control In this section we consider the synthesis of nonlinear control for tracking reference altitude and angular position trajectories. Let the angular = 0(t) and the altitude z = z0(t) reference signals be given as twice continuously di erentiable functions of time. Suppose that absolute values of the roll and the pitch at any time does not reach the value of =2 to avoid singularity of the C and G functional matrices de ned in this paper.

Introduce the tracking error variables ez = z z0(t), e = 0(t) and rewrite the equations ( 2 ) and ( 3 ) in the variables ez and e , respectively, as mz =

mg + F cos cos where = (x; y)T and z are coordinates of the vehicle center of mass in the inertial frame; , , are roll, pitch and yaw angles, respectively, = ( ; ; )T; m stands for the quadcopter mass, g is the acceleration due to gravity; F represents the thrust produced by the quadcopter rotors; M = (Mx; My; Mz)T is the vector of torques; ! = (!x; !y; !z)T is the vector of angular velocities in the body- xed frame, I = diag(Ix; Iy; Iz) is the diagonal inertia matrix, The control problem is to nd F and M such that

F m ez = g + cos cos

z0(t) e = C_ ! + CI 1M

CI 1!

I!

0(t): lim ez(t) = 0; t!+1

lim e (t) = 0: t!+1 ( 2 ) ( 3 ) ( 4 ) ( 5 ) Choose the stabilizing control laws as below

m F = (g + z0(t) cos cos

k2ez) ; M = !

I! + IC 1 0(t)

C_ !

C1e_

C2e ; where k1 > 0, k2 > 0 are positive gain coe cients and C1 > 0, C2 > 0 are positive de nite gain matrices. Then, the equations ( 4 ) and ( 5 ) with the controls ( 6 ) and ( 7 ), respectively, are written as ez + k1e_z + k2ez = 0;

e + C1e_ + C2e = 0; with the equilibrium point ez = 0, e = 0 being globally asymptotically stable.

One can take the gain coe cients k1 > 0, k2 > 0 and matrices C1 > 0, C2 > 0 to guarantee that jezj z if t tz and ke k if t t . Here z = 0:05ez(0), = 0:05e (0); tz and t are the desired transient times, respectively; k k stands for the Euclidian norm. As the desired characteristic polynomials of equations ( 8 ) one takes

Qz( ) = 2 + 2!z + !z2; respectively, where !z = 4:8=tz, ! = 4:8=t . This results in k1 = 2!z, k2 = !z2 and C1 = 2! E, C2 = !2E. Here, E is the identity matrix of size 3 3.

Figure 1 illustrates how the control laws ( 6 ) and ( 7 ) are applied to Parrot Minidrone ight control with the help of Simulink Support Package for Parrot Minidrones. For educational purposes we propose the following exercise. ( 6 ) ( 7 ) ( 8 ) (a) 0(t) = [0; 0; 0]T , z0(t) = 1; (b) 0(t) = z0(t) =

In both cases the following initial conditions are suggested: z(0) = z_(0) = 0, (0) = !(0) = 0.

Hint. In case (b) nd the coe cients ai to ful ll the initial and terminal conditions on z(t). 4

Horizontal and Vertical Position Control In the current section we deal with the design of nonlinear control for tracking reference position trajectories. Let the altitude z = z0(t) and the x; y position = 0(t) = [x0(t); y0(t)]T reference signals be four times continuously di erentiable.

For the convenience sake, introduce the new control variables (M~ x; M~ y; M~ z)T = M~ = C_ ! + CI 1 (M !

I!) ( 9 ) and rewrite the system ( 3 ) as

 = M~ :

Let a reference yaw trajectory 0(t) be given (see [5] for a discussion why to choose a reference yaw trajectory at this stage instead of a pitch or roll reference behavior). De ne the tracking error variable e = 0(t). Then, the yaw tracking control M~ z is written as

Mz = 0(t) ~ k3e_

k4e ; e + k3e_ + k4e = 0 where k3 > 0, k4 > 0 are some positive gain coe cients. Hence, the zero equilibrium of the closed-loop yaw error dynamics given by ( 10 ) ( 11 ) ( 12 ) is globally asymptotically stable.

Next, the system ( 1 ) with control ( 6 ) takes the form  = (g + z0(t) k1e_z

k2ez) cos tg sin tg + sin tg sec cos tg sec : (IV ) = cos sin

sin cos where fi are nonlinear scalar functions of the state.

Introduce the tracking error variable e = 0(t) and let G denote the matrix of coe cients of controls M~ x and M~ y in ( 12 ). Then, the x; y trajectory tracking control is written using nonlinear dynamics inversion as

i C4e : Here the gain matrices Ci > 0 are such that the zero equilibrium of the resulting closed-loop system given by

e( 4 ) + C1e( 3 ) + C2e + C3e_ + C4e = 0 is globally asymptotically stable.

Additionally, to ful ll the condition ke k and t is the required transient time, one takes if t t , where = 0:05e (0) as the desired characteristic polynomial of each equation of the system ( 14 ). This choice yields C1 = 4! E, C2 = 6!2E, C3 = 4!3E, C4 = !4E, where E is the identity matrix of size 2 2.

Figure 2 describes how the control laws ( 6 ), ( 10 ) and ( 13 ) are applied to Parrot Minidrone ight control using Simulink Support Package for Parrot Minidrones. Finally, the following exercise is suggested. ( 13 ) ( 14 )

Exercise 2. Design a nonlinear Parrot Minidrone control algorithm as a Simulink Subsystem block to track the following reference trajectories: (a) 0(t) = [0; 0]T , z0(t) = 1,

0(t) = 0. (b) 0(t) = [cos t; sin t]T , z0(t) = 1,

0(t) = t + =2.

The initial conditions are (0) = _(0) = 0, z(0) = z_(0) = 0, (0) = !(0) = 0.

Adaptive Control

This section considers the synthesis of nonlinear adaptive control for tracking reference altitude and angular position trajectories. The quadcopter mass m and components Ix, Iy, Iz of the diagonal inertia matrix I are treated as unknown constants.

Let J = [Ix; Iy; Iz]T and D( ), = [ 1; 2; 3]T be the diagonal matrix with the elements dii = i, i = 1; 2; 3. Then, the following equalities hold and the equations ( 3 ) can be written as !

I!

SI!; I! D(!)J;

0 0 !z !y 1 S = @ !z 0 !x A

!y !x 0 and introduce the error e! = ! 1, where 1 is the desired reference behavior of the ! variable to be de ned later. The time derivative of V1(e ) along the trajectories of the system ( 15 ) is given by

V_1(e ) = eT e_ = eT [Ce! + C 1 _0(t)] : ( 16 ) The choice 1 = C 1 _0(t) C 1K1e , where K1 > 0 is some positive de nite matrix, transforms ( 16 ) into the form

V_1(e ) = eT Ce! eT K1e :

Further, introduce the estimation error J~ = J J^, where J^ is an estimate for the unknown parameter vector J . Hence, since J is a constant vector, the following equality holds J~_ = J^_. To nd the tracking control M consider the function where a > 0 is a positive de nite matrix. The time derivative of V2(e ; e!; J~) along the trajectories of the system ( 15 ) is written as

V_2(e ; e!; J~) = eT e_ + eT! Ie_! + J~T a 1J~_ = eT Ce!

eT K1e + eT! (M =

eT K1e + eT! CT e + M J~T [SD(!) + D( _ 1)]T e!

SD(!)J

I _ 1) SD(!)J^

D( _ 1)J^

J~T a 1J^_ J~T a 1J^_; where _ 1 = C_ 1 _0(t) + C 1 0(t) C_ 1K1e unknown parameter estimation error J~ one takes C 1K1e_ . To eliminate the

Finally, the choice

where K2 > 0 is a positive de nite matrix, results in _ J^ =

a [SD(!) + D( _ 1)]T e!: M = SD(!)J^ + D( _ 1)J^

CT e

K2e!; V_2(e ; e!) = eT K1e eT! K2e! < 0: Therefore, for the system ( 15 ) in closed-loop form with the control ( 18 ) and parameter update law ( 17 ) by the LaSalle-Yoshizawa theorem holds the following: e (t) ! 0 as t ! +1.

Next, consider the reference altitude trajectory tracking adaptive control problem.

Let m^ be an estimate of the unknown quadcopter mass m. De ne also the error variable

The control problem is to nd F in ( 4 ) which guarantees that m~ = m

m^ : lim ez(t) = 0: t!+1

V1(ez) = 21 ez2 > 0 V_1(ez) = eze_z = ez ( + 2) :

V_1(ez) = ez c1ez2: ( 17 ) ( 18 ) (19)

To nd the stabilizing control F using the backstepping approach consider rst the function and introduce the error variable = e_z 2, where 2 is the desired reference behavior of e_z to be given later. The time derivative of V1(ez) is as follows The choice 2 = c1ez, where c1 > 0 is some positive constant, gives

Notice that the unknown coe cient 1=m of the thrust F in the system ( 4 ) can be written as 1 m = 1 m^ m~ mm^ : Additionally, since m is a constant, from (19) follows that m~_ = nd the tracking control F consider the function (20) m^_ . Finally, to V2 (ez; ; m~ ) = V1(ez) + 21 2 +

1 2kam m~ 2 > 0: Its time derivative along the trajectories of the system ( 4 ) in view of (20) is written as follows

V_2 (ez; ; m~ ) = eze_z + = ez

c1ez2 + To eliminate the unknown values of m and m~ one chooses

Finally, one takes

m^_ = ka

cos cos : F m^ F =

m^ cos cos -1 -2 0 15 20 air pressure ultrasonic 0.6 0.4 /s 0.2 d a r , y 0 , x -0.2 -0.4 -0.6

0 1 0.8 0.6 s / d ra 0.4 , z 0.2

0 -0.2 0 5 5 x (quadrotor) y (quadrotor) x & y (matlab) 1 (rad/s) versus time (s) (blue line) and

2 (rad/s) versus time (s) (blue line) and 5 5

Conclusion

This paper extended the functionality of the Simulink Support Package for Parrot Minidrones (SSPPM) by proposing nonlinear quadrotor control algorithms. We suggested using Parrot Minidrones together with the SSPPM as a nice and a ordable control laboratory equipment for nonlinear control education. Block diagrams illustrated how the control laws could be applied to Parrot Minidrone ight control. Exercises to design nonlinear Parrot Minidrone control algorithms as Simulink Subsystem blocks were suggested. The experimental results show rather good performance of the designed station-keeping control laws on the Parrot Mambo Minidrone.

References

1. Chen , F. , Zhang , K. , Wang , Z. , Tao , G. , Jiang , B. : Trajectory tracking of a quadrotor with unknown parameters and its fault-tolerant control via sliding mode fault observer . Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering 229 ( 4 ), 279 { 292 ( 2015 )

2. Dierks , T. , Jagannathan , S. : Output feedback control of a quadrotor UAV using neural networks . IEEE transactions on neural networks 21(1) , 50 { 66 ( 2010 )

3. Falconi , G.P. , Marvakov , V.A. , Holzapfel , F. : Fault tolerant control for a hexarotor system using Incremental Backstepping . In: IEEE Conference on Control Applications , pp. 237 { 242 ( 2016 )

4. Fantoni , I. , Lozano , R.: Non-linear control for underactuated mechanical systems . Springer-Verlag ( 2002 )

5. Glazkov , T.V. , Golubev , A.E. , Gorbunov , A.V. , Krishchenko , A.P. : Control of quadcopter motion in the horizontal plane . AIP Conference Proceedings 2116 , 380003 ( 2019 )

6. Golubev , A.E. , Thway , N. , Gorbunov , A.V. , Krishchenko , A.P. , Utkina N.V. : Construction of quadrocopter programmed motion in a at labyrinth . AIP Conference Proceedings 2116 , 380004 ( 2019 )

7. Khatoon , S. , Gupta , D. , Das , L. K. : PID & LQR control for a quadrotor: Modeling and simulation . In: 2014 International Conference on Advances in Computing Proceedings, Communications and Informatics , pp. 796 { 802 ( 2014 )

8. Krstic , M. , Kanellakopoulos , I. , Kokotovic , P.V. : Nonlinear and adaptive control design . John Wiley and Sons, New York ( 1995 )

9. Loianno , G. , Brunner , C. , McGrath , G. , Kumar , V. : Estimation, control, and planning for aggressive ight with a small quadrotor with a single camera and IMU . IEEE Robotics and Automation Letters 2 ( 2 ), 404 { 411 ( 2017 )

10. Luukkonen , T. : Modelling and control of quadcopter . Independent research project in applied mathematics , http://sal.aalto. /publications/pdf- les/ eluu11 public.pdf. Last accessed 14 Sept 2020

11. Madani , T. , Benallegue , A. : Backstepping control for a quadrotor helicopter . In: 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems Proceedings , pp. 3255 { 3260 ( 2006 )

12. Nguyen , N.T. , Prodan , I. , Stoican , F. , Lefevre , L. : Reliable nonlinear control for quadcopter trajectory tracking through di erential atness . IFAC-PapersOnLine 50 ( 1 ), 6971 { 6976 ( 2017 )

13. Rashid , M.I. , Akhtar , S. : Adaptive control of a quadrotor with unknown model parameters . In: 9th International Bhurban Conference on Applied Sciences and Technology Proceedings , pp. 8 { 14 ( 2012 )

14. Sanchez-Cuevas , P. , Heredia , G. , Ollero , A. : Characterization of the aerodynamic ground e ect and its in uence in multirotor control . International Journal of Aerospace Engineering . 2017 , ( 2017 )

15. Shi , D. , Wu , Z. , Chou , W. : Generalized extended state observer based high precision attitude control of quadrotor vehicles subject to wind disturbance . IEEE Access 6 , 32349 { 32359 ( 2018 )

16. Spedicato , S. , Notarstefano , G.: Minimum-time trajectory generation for quadrotors in constrained environments . IEEE Transactions on Control Systems Technology 26 ( 4 ), 1335 { 1344 ( 2018 )

17. Wang , R. , Liu, J. : Adaptive formation control of quadrotor unmanned aerial vehicles with bounded control thrust . Chinese Journal of Aeronautics 30 ( 2 ), 807 { 817 ( 2017 )

18. Simulink Support Package for Parrot Minidrones . https://www.mathworks.com/ help/supportpkg/parrot. Last accessed 14 Sept 2020