- Email: [email protected]
Trajectory Tracking for a Commercial Quadrotor via Time-Varying Backstepping
Proceedings, 2nd IFAC Conference on Proceedings, 2nd IFAC Conference on Modelling, Identification and Controlon of Nonlinear Systems Proceedings, 2nd IFAC Conference Modelling, Identification and Control of Nonlinear Systems Available online at www.sciencedirect.com Guadalajara, Mexico, June 20-22, 2018 Proceedings, 2nd IFAC Conference on Modelling, Identification and Control of Nonlinear Systems Guadalajara, Mexico, June 20-22, 2018 Modelling, Identification and Control of Nonlinear Systems Guadalajara, Mexico, June 20-22, 2018 Guadalajara, Mexico, June 20-22, 2018
ScienceDirect
IFAC PapersOnLine 51-13 (2018) 532–536
Trajectory Trajectory Tracking Tracking for for a a Commercial Commercial Trajectory Tracking for a Quadrotor via Time-Varying Backstepping Trajectory Tracking for a Commercial Commercial Quadrotor via Time-Varying Quadrotor via Time-Varying Backstepping Backstepping Quadrotor via Time-Varying Backstepping ∗ ∗ M. A. Rosaldo-Serrano ∗∗ E. Aranda-Bricaire ∗ ∗ M. A. Rosaldo-Serrano E. Aranda-Bricaire
M. A. Rosaldo-Serrano ∗∗ E. Aranda-Bricaire ∗∗ M. A. Rosaldo-Serrano ∗ E. Aranda-Bricaire ∗ ∗ M. A. Rosaldo-Serrano E. Aranda-Bricaire ∗ ∗ CINVESTAV, CINVESTAV, Electrical Electrical Engineering Engineering Department, Department, Mechatronics Mechatronics ∗ Section, ∗ AP 14-740, 07000 Ciudad de Mexico, Mexico (e-mail: CINVESTAV, Electrical Engineering Department, Mechatronics ∗ Section, AP 14-740, 07000 Ciudad de Mexico, Mexico (e-mail:
CINVESTAV, Electrical Engineering Mechatronics [email protected], Section, AP 14-740, 07000 [email protected]). deDepartment, Mexico, Mexico (e-mail: [email protected], Section, AP 14-740, 07000 [email protected]). de Mexico, Mexico (e-mail: [email protected], [email protected]). [email protected], [email protected]). Abstract: Abstract: In In this this paper paper we we propose propose aa non-linear non-linear control control law law to to achieve achieve trajectory trajectory tracking tracking in in the 3D space for a commercial AR.Drone 2.0 quadrotor. The proposed control law is based on Abstract: In this paper we propose a non-linear control law to achieve trajectory tracking in the 3D space for a commercial AR.Drone 2.0 quadrotor. The proposed control law is based on Abstract: Intime thisa varying paper we propose athe non-linear controltechnique law proposed to achieve trajectory tracking in a non-linear version of Backstepping that takes into account the the 3D space for commercial AR.Drone 2.0 quadrotor. The control law is based on athenon-linear time version of the Backstepping technique that control takes into account the 3D space for internal a varying commercial AR.Drone 2.0 quadrotor. The proposed law is based on dynamics of the orientation controller of the AR.Drone. The stability of the control a non-linear time varying version of the Backstepping technique that takes into account the dynamics of the internal orientation of the AR.Drone. The stability the control a non-linear time varying version of controller the Lyapunov Backstepping technique that takes intoof the law is ensured by the design of recursive functions and its is tested by dynamics of the internal orientation controller of the AR.Drone. Theperformance stability of account the control law is ensured by the design of recursive Lyapunov functions and its performance is tested by dynamics of the internal orientation controller of the AR.Drone. The stability of the control numeric simulations on MATLAb Simulink environment. law is ensured by the design of recursive Lyapunov functions and its performance is tested by numeric simulations on MATLAb Simulink environment. law is ensured by theondesign of recursive Lyapunov functions and its performance is tested by numeric simulations MATLAb Simulink environment. © 2018, IFAC (International FederationSimulink of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. numeric simulations on MATLAb environment. Keywords: control Keywords: Quadrotors, Quadrotors, Trajectory Trajectory tracking, tracking, Backstepping, Backstepping, non-linear non-linear control Keywords: Quadrotors, Trajectory tracking, Backstepping, non-linear control Keywords: Quadrotors, Trajectory tracking, Backstepping, non-linear control 1. INTRODUCTION well a dynamical model for this internal controller. We We 1. INTRODUCTION well a dynamical model this internal controller. base the modelingmodel of thefor internal controller on a paper 1. INTRODUCTION well a dynamical for this internal controller. We base the modeling of the internal controller on a paper 1. INTRODUCTION athe dynamical model this the internal controller. We by Bristeau et al. (2011) where AR.Drone base modeling of thefor internal controller on ainternal paper While many possible types of small Unmanned Aerial Ve- well by Bristeau et al. (2011) where the AR.Drone internal While many possible types of smallhelicopter Unmanned Aerial Ve- base the modeling of the internal controller on ainternal paper programming is presented, however most parameters are hicles (UAV) exist, the quadrotor have become by Bristeau et al. (2011) where the AR.Drone While many possible types of small Unmanned Aerial Veprogramming presented, however most parameters are hicles (UAV) exist, the quadrotor helicopter have become by Bristeau Specifically, etis al. (2011)according where the AR.Drone internal undisclosed. to said paper, we asWhile (UAV) many possible types of small Unmanned Aerial Vepopular in research applications because of its wide area of programming is presented, however most parameters are hicles exist, the quadrotor helicopter have become undisclosed. Specifically, according to said paper, we aspopular in research applications because of its wide area of programming is presented, however most parameters are sume the angular dynamics under the internal controller hicles (UAV) exist, the quadrotor helicopter have become undisclosed. Specifically, according to said paper, we aspossible in applications, as seen in because Gupte etofal. This popular research applications its (2012). wide area of sume the angular dynamics under the internal controller possible applications, as seen in Gupte et al. (2012). This undisclosed. Specifically, according to said paper, we asobey second order transfer functions. Then the parameters popular in research applications because of its wide area of sume the angular dynamics under the internal controller is due to its small size and weight, and its ability to move possible applications, as seen in Gupte et al. (2012). This obey second order transfer functions. Then the parameters is due to its smallinside sizeas and weight, and its ability toand move sume the angular dynamics under the internal controller of such transfer functions are identified through the least possible applications, seen indimensional Gupte et al. (2012). This in any direction a three space its obey second order transfer functions. Then the parameters is due to its small size and weight, and its ability to move such transfer functions are identified through the least in any direction inside a three dimensional spacetoand its of second order transfer functions. Then thedata. parameters squares method by the analysis of navigation is due its small size and weight, and ability move ability to hover and vertical take-off anditslanding. of such transfer functions are identified through the least in any to direction inside a three dimensional spaceRegardand its obey squares method by the analysis of navigation data. ability to hover and vertical take-off and landing. Regardof such transfer functions are identified through the least in any direction inside a three dimensional space and its squares method by the analysis of navigation data. less of its many and advantages one of the drawbacks ability to hover vertical take-off andbiggest landing. RegardWe have previous works about the trajectory tracking less ofquadrotor its many and advantages one of the biggest drawbacks squares method by the analysis of navigation data. We have previous works about the trajectory tracking ability to hover vertical take-off and landing. Regardof a is its reduced payload which also limits less of its many advantages one of the biggest drawbacks problem for one quadrotor, see Santiaguillo-Salinas et of a quadrotor is its reduced payload which also limits We have previous works about the trajectory tracking problem for one quadrotor, see Santiaguillo-Salinas et al. al. lessaamount ofquadrotor its many advantages one ofbethe biggest drawbacks the of batteries that can carried and its flight of is its reduced payload which also limits We haveand previous works about the see trajectory tracking (2017), a group of quadrotors, Rosaldo-Serrano the amount of batteries that can be carried and its flight problem for one quadrotor, see Santiaguillo-Salinas et al. and a group of quadrotors, see Rosaldo-Serrano of aamount quadrotor is its isreduced payload which also time. The quadrotor athat system with four propellers in (2017), the of batteries can be carried and its limits flight for one quadrotor, see Santiaguillo-Salinas et al. et al. said we based (2017), and aIn group of quadrotors, see controllers Rosaldo-Serrano time. The quadrotor is athat system with four and propellers in problem al. (2016). (2016). said works works we designed designed controllers based amount ofconfiguration. batteries can be carried athe cross shape While the front and its theflight rear time. The quadrotor is a system with four propellers in et (2017), and order aIn group of quadrotors, see of Rosaldo-Serrano on the first linear approximation the AR.Drone amotor cross shape configuration. While the front and the rear et al. (2016). In said works we designed controllers based the(2016). first order linear approximation of the AR.Drone quadrotor is the a system four propellers in on rotate clockwise, left andwith the front right motor rotate atime. crossThe shape configuration. While the and the rear et al. Inmain said works we designed controllers based dynamics. contribution of this paper is motor the left and the front right motor rotate the firstThe order linear approximation of the AR.Drone dynamics. The main contribution of this paper is the the dedea crossrotate shapeclockwise, configuration. While the and the rear on counter-clockwise which nearly cancels gyroscopic effects motor rotate clockwise, the left and the right motor rotate on the first order linear approximation ofpaper the AR.Drone sign of a nonlinear control strategy for trajectory tracking counter-clockwise which nearly cancels gyroscopic effects dynamics. The main contribution of this is the deof a nonlinear control strategy for trajectory tracking motor rotate clockwise, and the right motor rotate sign and aerodynamic torques in left trimmed flight. One additional counter-clockwise whichthe nearly cancels gyroscopic effects dynamics. The main contribution offorthis paper isthe thePardeof a quadrotor in the 3D space, specifically, for and aerodynamic torques in trimmed flight. One additional sign of a nonlinear control strategy trajectory tracking a of quadrotor in the 3D space, specifically, for the Parcounter-clockwise which nearly cancels gyroscopic effects of advantage of the torques quadrotor compared to aOne conventional and aerodynamic in trimmed flight. additional sign a nonlinear control strategy for trajectory tracking rot’s AR.Drone 2.0.the The proposed control strategy consist advantage of the quadrotor compared to a conventional of a quadrotor in 3D space, specifically, for the Parand aerodynamic in trimmed flight. additional AR.Drone in 2.0. The control strategy consist helicopter is simplified rotor mechanics. By varying rot’s advantage of the the torques quadrotor compared to aOne conventional of a quadrotor the 3Dproposed space, specifically, for the Parnon-linear time-varying version of the backstepping helicopter is the simplified rotor mechanics. By varying rot’s AR.Drone 2.0. The proposed control strategy consist advantage the quadrotor compared to aapplied conventional of a non-linear time-varying version of the backstepping the thrust of a single motor, the lift force to the helicopter is the simplified rotor mechanics. By varying rot’s AR.Drone 2.0. Thethe proposed control strategy consist technique. To validate theoretical results we present the thrust of a single motor, the lift force applied to the of a non-linear time-varying version of the backstepping To validate the theoretical we present helicopter is simplified mechanics. By varying quadrotor manipulated to produce and/or the thrust can of the abe single motor,rotor the lift force vertical applied to the technique. of a non-linear time-varying version ofresults the backstepping numerical simulations on the MATLab simulink quadrotor can be manipulated to produce vertical and/or technique. To validate the theoretical results weenvironpresent numerical simulations on the MATLab simulink the thrust of a single motor, the lift force applied to the lateral motion. In spite of the four the quadrotor quadrotor can be manipulated to actuators, produce vertical and/or technique. To validate thethe theoretical results weenvironpresent ment. The main advantage of the controller proposed in lateral motion. In spite of the four actuators, the quadrotor numerical simulations on MATLab simulink environThe simulations main advantage ofMATLab the controller proposed in quadrotor can be manipulated to with produce vertical and/or ment. is aa dynamically system nonlinear dynamics lateral motion. Inunstable spite of the four actuators, the quadrotor numerical on the simulink environthis paper is its easy implementation and proven stability, is dynamically unstable system with nonlinear dynamics ment. The main advantage of the controller proposed in lateral motion. In spite of the four actuators, the algorithm. quadrotor this paper is its easy implementation and proven stability, that has to be stabilized by a suitable control is a dynamically unstable system with nonlinear dynamics ment. Thethe advantage of Lyapunov the controller proposed in based on recursive use of functions, while that has to be stabilized by a suitable control algorithm. this paper ismain its easy implementation and proven stability, based on the recursive use of Lyapunov functions, while is a dynamically unstable system with nonlinear dynamics Manyhas of to thebeproposed for control quadrotors, as in this that stabilizedcontrollers by a suitable algorithm. paper isnon-linearities its easy implementation and proven stability, taking the of the dynamics directly into Many of the proposed controllers for quadrotors, as in based on the recursive use of Lyapunov functions, while thethe non-linearities the dynamics directlywhile into that has stabilized by a suitable control algorithm. Wang et al. (2016), arecontrollers based on aforlinearized model of taking Many of to thebe proposed quadrotors, as in on recursive useof Lyapunov functions, account. Wang et arecontrollers based on aforlinearized model of based taking the non-linearities ofof the dynamics directly into account. Many of al. the (2016), proposed quadrotors, as in the quadrotor. Wang et al. (2016), are based on a linearized model of taking the non-linearities of the dynamics directly into the quadrotor. Wang et al. (2016), are based on a linearized model of account. This the quadrotor. account. This paper paper is is organized organized as as follows. follows. Section Section 2 2 presents presents For many research groups it is common to use commercial the quadrotor. the nonlinear dynamical model of the AR.Drone 2.0. For many research groups it is common to use commercial This paper is organized as follows. Section 2 presents the nonlinear dynamical model of the AR.Drone 2.0. quadrotors due to their accessible prices and reliability. For many research groups it is common to use commercial This nonlinear paper isthe organized as follows. Section 2 presents obtained by combination of the classic model of quadrotors due to their accessible prices and reliability. the dynamical model of the AR.Drone 2.0.a obtained by the combination of the classic model of many research groups it decided is common to use commercial For this project, our group to use the AR.Drone quadrotors due to their accessible prices and reliability. the nonlinear dynamical model of the AR.Drone 2.0.a generic quadrotor and the identified internal controller For this project, our group decided to use the AR.Drone obtained by the combination of the classic model of a generic quadrotor and the identified internal controller quadrotors due to their accessible prices and reliability. 2.0 quadrotor manufactured by Parrot. As seen in Krajnk obtained For this project, our group decided to use the AR.Drone by the combination of3 the classic model of of a of the AR.Drone 2.0 . Section presents the design 2.0 quadrotor manufactured by Parrot. As seen in Krajnk generic quadrotor and the identified internal controller the AR.Drone 2.0 . Section 3 presents the controller design of Foral. this project, group decided to use the AR.Drone et (2011), theour AR.Drone a popular choice among of 2.0 quadrotor manufactured byis Parrot. As seen in Krajnk generic quadrotor and the identified internal the nonlinear control strategy for 3D trajectory tracking. et (2011), the AR.Drone a popular choice among the AR.Drone 2.0 strategy . Sectionfor3 3D presents the design of the nonlinear control trajectory tracking. 2.0 al. quadrotor manufactured byis Parrot. Aslarge seen in Krajnk researchers because of its low and number of of et al. (2011), the AR.Drone iscost a popular choice among of the AR.Drone 2.0numeric . Section 3 3D presents the design of Section 44 presents simulations to validate the researchers because of its low cost and large number of the nonlinear control strategy for trajectory tracking. Section presents numeric simulations to validate the et al. (2011), the The AR.Drone a2.0, popular choice among sensors on board. as many other comresearchers because of AR.Drone its lowiscost and large number of the nonlinear control strategy for 3D trajectory tracking. designed control strategy. Finally, in Section 5 we present Section 4 presents numeric simulations to validate the sensors on board. The AR.Drone 2.0, as many other comstrategy. Finally, in Section we present researchers because of AR.Drone itsanlow cost large other number of designed mercial quadrotors, have internal stabilizing controller sensors on board. The 2.0,and as many comSection 4control presents numeric simulations to 55validate the conclusions and possible future work. mercial quadrotors, have an internal stabilizing controller designed control strategy. Finally, in Section we present conclusions and possible future work. sensors on board. The AR.Drone 2.0, as many other comfor its Euler angles. For the correct implementation of mercial quadrotors, have an internal stabilizing controller designed control strategy. Finally, in Section 5 we present for its Euler angles.have For an theinternal correct implementation of conclusions and possible future work. mercial quadrotors, stabilizing controller our proposed trajectory controller we for its Euler angles. Fortracking the correct implementation of conclusions and possible future work. our proposed trajectory tracking controller we propose propose as as for its Euler angles. For the correct implementation of our proposed trajectory tracking controller we propose as our proposed trajectory tracking controller we propose as This work was financial supported by CONACyT, Mexico, through
This work was financial supported by CONACyT, Mexico, through theThis scholarship work was280703. financial supported by CONACyT, Mexico, through the scholarship 280703. This work was280703. financial supported by CONACyT, Mexico, through the scholarship the scholarship 280703. 2405-8963 © 2018, IFACConference (International Proceedings, 2nd IFAC onFederation of Automatic Control) 532 Hosting by Elsevier Ltd. All rights reserved. Proceedings, 2nd IFAC Conference on 532 Control. Peer reviewIdentification under responsibility of International Federation of Automatic Modelling, and Control of Nonlinear Proceedings, 2nd IFAC Conference on 532 Modelling, Identification and Control of Nonlinear 10.1016/j.ifacol.2018.07.334 Systems Proceedings, 2nd IFAC Conference on 532 Modelling, Systems Identification and Control of Nonlinear Guadalajara, Mexico, June 20-22, 2018 Modelling, Identification and Control of Nonlinear
2018 IFAC MICNON Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
533
Using the above equations, we can represent the dynamical model of a generic quad-rotor helicopter as u x ¨=− (cos ψ sin θ cos φ + sin ψ sin φ) (4a) m u y¨ = − (sin ψ sin θ cos φ − cos ψ sin φ) (4b) m u z¨ = − (cos θ cos φ) + g (4c) m (4d) ψ¨ =˜ τψ ¨ (4e) θ =˜ τθ (4f) φ¨ =˜ τφ τθ , where the total thrust u and the angular momenta τ˜ψ ,˜ and τ˜φ are the control inputs of the system. 2.2 Modeling of the AR.Drone 2.0 internal controller Fig. 1. Quad-rotor helicopter scheme. 2. DYNAMICAL MODEL OF THE AR.DRONE 2.0 2.1 Dynamical Model for a generic quadrotor Consider a quadrotor helicopter moving in a three dimensional space. We define a North-East-Down (NED) inertial frame Fe fixed to earth and a xyz body frame Fb fixed to the center of mass of the quadrotor helicopter, as shown in Fig. 1. The position and orientation of the body frame Fb with respect to the fixed frame Fe are given by T T ξ = [x, y, z] and η = [ψ, θ, φ] , respectively. Where x,y and z are the spatial coordinates of the quadrotor center of mass and ψ, θ, and φ are the respective Euler angles of yaw, pitch and roll of the quadrotor body frame. Based on Garca-Carrillo et al. (2013), the nonlinear dynamics of the quad-rotor helicopter can be expressed as mξ¨ = − mgD + RF (1) ˙ = − Ω × IΩ + τ IΩ
(2)
T
where D = [0, 0, −1] , R ∈ SO(3) is a rotation matrix 4 T relating Fe with Fb , F = [0, 0, u] with u = i=1 Fi is the total thrust applied to the quadrotor by the four rotors, m is the total mass of the quadrotor, g is the standard acceleration due to gravity, I is the inertia matrix of the quadrotor in body frame, and τ is the total torque applied to the quadrotor by the four rotors. The variable Ω = T [r, q, p] represents the angular velocity of the quadrotor defined by Ω = W η˙ − sin θ 0 1 W = cos θ sin φ cos φ 0 . cos θ cos φ − sin φ 0 Demoting cos(∗) and sin(∗) as c∗ and s∗ , respectively, the rotation matrix R ∈ SO(3) is given by c θ cψ sφ sθ cψ − cφ sψ cφ sθ cψ + sφ sψ R = cθ sψ sφ sθ sψ + cφ cψ cφ sθ sψ − sφ cψ −sθ sφ cθ cφ cθ We define an auxiliary vector τ˜ related to generalized torque τ and based on eq. (2) as τ˜ψ ˙ η˙ − W η˙ × IW η˙ + τ ) (3) τ˜ = τ˜θ = I −1 W −1 (−I W τ˜φ 533
In Bristeau et al. (2011) the authors show the main lines of the internal controller embedded in the AR.Drone 2.0, which stabilizes the angular dynamics. However, the actual parameters of the internal controller remain undisclosed. We assume that the dynamics of the internal controller of the AR.Drone can me modeled by the following differential equations (5a) z¨ = − a1 z˙ + a3 uz ˙ ¨ (5b) φ = − b1 φ − b2 φ + b3 uφ θ¨ = − c1 θ˙ − c2 θ + c3 uθ ψ¨ = − d1 ψ˙ + d3 uψ
(5c)
(5d) where the control inputs of the AR.Drone are the desired roll and pitch angles uφ and uθ , the desired ascending velocity uz and the desired rotational velocity for yaw uψ . The parameters ai , bi , ci and di were calculated by analyzing the data obtained from several experiments performed with step inputs. The data obtained was processed with the MATLab software using the least square method from its system identification toolbox. Is noteworthy that the AR.Drone doesn’t allow the direct control of the total thrust u. To obtain the complete non-linear model for the AR.Drone we compute the total thrust u from (4c) and substitute it in (4a) and (4b). Then we add the equations for the internal controller of the AR.Drone we obtained in (5a)-(5d). Finally, we obtain tan φ sin ψ (6a) x ¨ =(−a1 z˙ + a3 uz − g) tan θ cos ψ + cos θ tan φ cos ψ (6b) y¨ =(−a1 z˙ + a3 uz − g) tan θ sin ψ − cos θ z¨ = − a1 z˙ + a3 uz (6c) ˙ ¨ (6d) φ = − b1 φ − b2 φ + b3 uφ θ¨ = − c1 θ˙ − c2 θ + c3 uθ ψ¨ = − d1 ψ˙ + d3 uψ
(6e)
(6f) Notice that the control inputs uz , uθ , uφ and uψ for the complete system are the same as in the internal controller of the AR.Drone. 3. NON-LINEAR CONTROL DESIGN 3.1 Altitude and yaw orientation control Our purpose is to design a control algorithm to achieve trajectory tracking for a quadrotor in 3D space. First we
2018 IFAC MICNON 534 Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
consider that the dynamics of the altitude and yaw angle of our quadrotor, given by (6c) and (6f), are decoupled for the rest of the (6a)-(6f) system and propose a feedforward PD controller to control them. Define the states for the altitude and yaw dynamics, respectively, by z1 =z (7a) (7b) z2 =z˙ ψ1 =ψ (7c) (7d) ψ2 =ψ˙ and define the tracking errors by ez1 =z1 − z1d (8a) ez2 =z2 − z˙1d
(8b)
eψ2 =ψ2 −
(8d)
eψ1 =ψ1 − z1d (t)
ψ1d ψ˙ 1d
(8c)
where is the desired trajectory for the altitude z1 and ψ1d (t) is the desired trajectory for the yaw angle ψ1 . For the control of the altitude and yaw dynamics we propose the PD controllers given by 1 a1 z2 − kpz ez1 − kdz ez2 + z¨1d (9) uz = a3 1 d1 ψ2 − kpψ eψ1 − kdψ eψ2 + ψ¨1d (10) uψ = d3 where a1,3 and d1,3 are parameters we obtained while modeling the AR.Drone internal dynamics for the altitude and yaw orientation and kpz , kdz , kpψ and kdψ are the gains of the controllers. Since the altitude and yaw dynamics are decoupled from the rest of the system, it follows straightforwardly that the states z1 and ψ1 converge exponentially to their desired trajectories z1d and ψ1d , respectively.
ξ˙2 =gz (t)A(ψ1 )η1∗ η˙ 1∗ =η2∗ η˙ ∗ =M˙ η1 η2 + Mη1 (−β1 η2 − β2 η1 + β3 uη ) 2
For the design of the controller for the quadrotor motion in the XY plane we propose a time-varying non-linear backstepping controller based on Kokotovic (1992) and Khalil (2002). Define the states for the X, Y , θ and φ dynamics as ξ1 = [x, y]
T
(11a)
ξ2 = [x, ˙ y] ˙
T
(11b)
T
(11c) (11d)
Using these states we can rewrite the nonlinear system (6a),(6b),(6d),(6e) as ξ˙1 =ξ2 (12a) tan η 11 ξ˙2 =gz (t)A(ψ1 ) (12b) tan η12 / cos η11 η˙ 1 =η2 (12c) η˙ 2 = − β1 η2 − β2 η1 + β3 u (12d) ci 0 where βi = for i = 1, 2, 3, are constant matrices 0 bi corresponding to the parameters obtained in the modeling T of the AR.Drone internal controller, u = [uθ , uφ ] are 534
(13b) (13c) (13d)
Using the auxiliary controller ˙ η1 η2 + vη ) uη = β3−1 β1 η2 + β2 η1 + Mη−1 ( M 1 the system (13a)-(13d) is reduced to ξ˙1 =ξ2
ξ˙2 =gz (t)A(ψ1 )η1∗ η˙ 1∗ =η2∗ η˙ 2∗ =vη Define now the system errors as eξ1 =ξ1 − ξ1d eξ2 =ξ2 − ξ˙d 1
eη1 =η1∗ − η1∗d eη2 =η2∗
ξ1d
3.2 Position control in the XY plane
η1 = [θ, φ] T ˙ φ˙ η2 = θ,
the control inputs for the system, and gz (t) = −a1 z2 + cos ψ1 sin ψ1 a3 uz − g and A(ψ1 ) = are considered as sin ψ1 − cos ψ1 time-varying parameters. We apply a coordinate change to (11a)-(11d), tan η11 , η1∗ = tan η12 / cos η11 η2∗ = Mη1 η2 were sec2 η11 0 Mη1 = sec η11 tan η11 tan η12 sec η11 sec2 η12 and obtain the new system (13a) ξ˙1 =ξ2
−
η˙ 1∗d
(14a) (14b) (14c) (14d) (15a) (15b) (15c) (15d)
where is the desired trajectory of the agent in the XY plane and η1∗d is a function of the desired acceleration ξ¨1d given by d tan η11 η1∗d = d d / cos η11 tan η12 η1d = tan−1 (gz−1 (t)Aθψ (ψ1 )ξ¨1d ) cos ψ sin ψ with Aθψ = . The dynamics d d cos η11 sin ψ − cos η11 cos ψ of the system in error coordinates become (16a) e˙ ξ1 =eξ2 e˙ ξ2 =gz (t)A(ψ1 )eη1 (16b) (16c) e˙ η1 =eη2 e˙ η2 =vη − η¨1∗d (16d) Rewriting the error dynamics in matrix form and defining vη = η¨1∗d + wη we have e˙ ξ =Aξ eξ + Bξ (t)eη1 (17a) e˙ η1 =eη2 (17b) (17c) e˙ η2 =wη T
where wis thecontrol input of the system, eξ = [eξ1 , eξ2 ] , 0 I2 Aξ = is a constant matrix with I2 ∈ R2×2 0 0 0 the identity matrix and Bξ (t) = gz (t) is a A(ψ1 )
2018 IFAC MICNON Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
time-varying matrix depenfing on the known time-varying funtions gz and A(ψ1 ). We can note the system (17a)(17c) is in strict feedback form, meaning that we can use the backstepping algorithm to stabilize it. To begin the backstepping design process, we need a control law able to stabilize the subsystem (17a). Considering eη1 as the virtual input of the subsystem we propose the non-linear control law eη1 = α1 (eξ , t) given by tan−1 (γ1 ) α1θ = (18) α1 (eξ , t) = α1φ tan−1 (γ2 cos α1θ ) where γ1 = gz−1 A(ψ1 )−1 [−kp eξ1 − kd eξ2 ] γ2
were kp , kd > 0 are the controller gains. Is noteworthy that the control law (18) is bounded. The closed-loop system (17a)-(18) produces (19) e˙ ξ = Heξ with 0 1 ⊗ I2 H= −kp −kd where ⊗ denotes the Kronecker product. Now we propose the following Lyapunov function candidate 1 (20) V1 = eTξ P eξ 2 where the matrix P has the form a b (21) P = ⊗ I2 . b c taking a = 1 + kp−1 kd−1 kp + kd kp−1 , b = kp−1 and c = 1 + kp−1 kd−1 . It follows that 1 1 (22) V˙ 1 = eTξ P H + H T P eξ = − eTξ Qeξ < 0 2 2 for all eξ = 0. Therefore the closed-loop system (17a) with the virtual control input (18) is exponentially stable. Now that we have a known stabilizing control law and Lyapunov function for the subsystem (17a) we can start the backstepping procedure. First, take the subsystem (17a)-(17b) ˜ξ eη2 e˜˙ ξ = A˜ξ (t)˜ eξ + B (23) eξ A B (t) ξ ξ ˜ξ = 0 . , A˜ξ (t) = and B where e˜ξ = I2 0 0 eη1 Taking eη2 as a virtual input, we expand the stabilizing properties of (18) to (23) by the backstepping process. For this first step of backstepping, we define a virtual state y2 given by the difference between the real state eη1 and the virtual control law α1 y2 = eη1 − α1 (eξ , t) (24) y˙ 2 = eη2 − α˙ 1 (eξ , t) (25) changing the subsystem to the coordinates of y2 and using the virtual control law eη2 = α˙ 1 (eξ , t) + u1 we get (26a) e˙ ξ = [Aξ eξ + Bξ (t)α1 (eξ , t)] + Bξ (t)y2 y˙ 2 =u1 (26b) Consider the Lyapunov function candidate 1 (27) V2 = V1 + y2T y2 2 and its derivative 1 (28) V˙ 2 ≤ − eTξ Qeξ + y2T [BξT (t)P eξ ] + y2T u1 2 535
535
we choose the control law (29) u1 = −[BξT (t)P eξ ] − k1 y2 with k1 > 0 as the control gain, to obtain 1 (30) V˙ 2 ≤ − eTξ Qeξ − y2T k1 y2 2 Therefore the subsystem (26a)-(26b)-(29) is exponentially stable. Substituting u1 , α˙ 1 (eξ , t) and y2 in the virtual control eη2 = α2 (eξ , eη1 , t) we have α2 =α˙ 1 − BξT (t)P eξ − k1 (eη1 − α1 (eξ ))
(31)
Finally we expand the stabilizing properties of (31) to the control law w for the complete system (16a)-(16d). For this second step of backstepping, we define the virtual variable y3 as y3 = eη2 − α2 (eξ , eη1 , t) (32) (33) y˙ 3 = w − α˙ 2 (eξ , eη1 , t) changing the system to the coordinates of y3 and using the virtual control law w = α˙ 2 (eξ , eη1 , t) + u2 we get ˜ξ α2 (eξ , eη1 , t) + B ˜ ξ y3 e˜˙ ξ = A˜ξ (t)˜ eξ + B (34a)
y˙ 3 =u2 (34b) Consider the Lyapunov function candidate 1 (35) V3 = V2 + y3T y3 2 and its derivative 1 (36) V˙ 3 ≤ − eTξ Qeξ − y2T k1 y2 + y3T y2 + y3T u2 2 we choose the control law (37) u2 = −y2 − k2 y3 with k2 > 0 as the control gain, to obtain 1 (38) V˙ 3 ≤ − eTξ Qeξ − y2T k1 y2 − y3T k2 y3 2 so that the subsystem (34a)-(34b)-(37) is exponentially stable. Substituting u2 , α˙ 2 (eξ , eη1 , t) and y3 in the control law w we obtain the control law wη =α˙ 2 (eξ , eη1 ) − (eη1 − α1 (eξ )) − k2 (eη2 − α2 (eξ , eη1 )) (39) for the motion control of the quadrotor in the XY plane. 4. NUMERICAL SIMULATIONS
To validate the designed control law (39) we perform numerical simulations on MATLab Simulink environment. The dynamical model of the AR.Drone 2.0 was programmed using the parameters obtained from the experimental analysis we did for the modeling of its internal controller. We consider that all the states are known and can be tracked at all times. The desired trajectory in the XY plane is defined as a Lemniscate of Gerono given by xd = sin(wt) y d = sin(wt) cos(wt) while the desired altitude and yaw trajectories are defined by z d = 1 + 0.2 sin(wt) ψ d = sin(wt) with w = 2π 60 for all the trajectories. Figure 2 shows the trajectory tracking by the quadrotor in the 3D space.
2018 IFAC MICNON 536 Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
Figures 3 and 4 show the respective position errors in X, Y and Z, and the orientation errors in θ, φ and ψ. The error graphics were trimmed to 10 seconds to be able see the errors with more detail at the transitory time, even when the desired trajectory needs 60 seconds to complete a full loop. We can note all of the errors converge exponentially to 0 in a short time. Trajectory control
the control law was proven by the design of adequate Lyapunov functions and its performance was tested by numeric simulations on MATLab Simulink software. As future work we propose the real-time implementation of the designed control law on an AR.Drone 2.0, the design of non-linear observers for the estimation of its linear and angular velocities. Also we consider the possibility to expand the designed control law for the control of a multiagent system with quadrotors as the system agents.
1.5
z position (m)
REFERENCES 1
Bristeau, P.J., Callou, F., Vissire, D., and Petit, N. (2011). The navigation and control technology inside the ar.drone micro uav. 18th IFAC World Congress, Milano, Italy, 1477–1484. Garca-Carrillo, L.R., Dzul-Lpez, A.E., Lozano, R., and Pgard, C. (2013). Modeling the Quad-Rotor MiniRotorcraft. Springer, London. Gupte, S., Mohandas, P., and Conrad, J. (2012). A survey of quadrotor uavs. 2012 Proceedings of IEEE Southeastcon, Orlando, FL, USA. Khalil, H.K. (2002). Nonlinear Systems. Prentice Hall. Kokotovic, P.V. (1992). The joy of feedback: nonlinear and adaptive. IEEE Control Systems Magazine, 12, 7–17. Krajnk, T., Vonsek, V., Fiser, D., and Faigl, J. (2011). Ardrone as a platform for robotic research and education. International Conference, Prague, Czech Republic, 172– 186. Rosaldo-Serrano, M.A., Santiaguillo-Salinas, J., and Aranda-Bricaire, E. (2016). Observer-based timevarying backstepping control for a quadrotor multiagent system. 2017 International Conference on Unmanned Aircraft Systems ICUAS’17, Miami, FL, USA, 1324–1332. Santiaguillo-Salinas, J., Rosaldo-Serrano, M.A., and Aranda-Bricaire, E. (2017). Observer-based timevarying backstepping control for parrots ar.drone 2.0. 20th IFAC World COngress, Toulouse, France, 10794– 10799. Wang, P., Man, Z., Cao, Z., Zheng, J., and Zhao, Y. (2016). Dynamics modelling and linear control of quadcopter. International Conference on Advanced Mechatronic Systems, Melbourne, VIC, Australia.
0.5
0 1 0.5 0 −0.5
y position (m)
0
−0.5
−1
Desired trajectory 0.5 Real trajectory
1
x position (m)
Fig. 2. Desired and actual trajectories Error (m)
Position Error (x) 0.2 0 −0.2
0
1
2
3
4
5
6
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
Error (m)
Time (s) Position Error (y) 0.2 0 −0.2
0
1
2
3
4
5
6
Error (m)
Time (s) Position Error (z) 2 0 −2
0
1
2
3
4
5
6
Time (s)
Error (rad)
Error (rad)
Error (rad)
Fig. 3. Position Errors Orientation Error (θ) 0.05 0 −0.05
0
1
2
3
4
5
6
Time (s) Orientation Error (φ) 0.01 0 −0.01
0
1
2
3
4
5
6
Time (s) orientation Error (ψ) 0.05 0 −0.05
0
1
2
3
4
5
6
Time (s)
Fig. 4. Orientation Errors 5. CONCLUSIONS AND FUTURE WORK We successfully designed a non-linear control law able to achieve trajectory tracking in the 3D space for a commercial AR.Drone 2.0 quadrotor. The stability of 536
ScienceDirect
IFAC PapersOnLine 51-13 (2018) 532–536
Trajectory Trajectory Tracking Tracking for for a a Commercial Commercial Trajectory Tracking for a Quadrotor via Time-Varying Backstepping Trajectory Tracking for a Commercial Commercial Quadrotor via Time-Varying Quadrotor via Time-Varying Backstepping Backstepping Quadrotor via Time-Varying Backstepping ∗ ∗ M. A. Rosaldo-Serrano ∗∗ E. Aranda-Bricaire ∗ ∗ M. A. Rosaldo-Serrano E. Aranda-Bricaire
M. A. Rosaldo-Serrano ∗∗ E. Aranda-Bricaire ∗∗ M. A. Rosaldo-Serrano ∗ E. Aranda-Bricaire ∗ ∗ M. A. Rosaldo-Serrano E. Aranda-Bricaire ∗ ∗ CINVESTAV, CINVESTAV, Electrical Electrical Engineering Engineering Department, Department, Mechatronics Mechatronics ∗ Section, ∗ AP 14-740, 07000 Ciudad de Mexico, Mexico (e-mail: CINVESTAV, Electrical Engineering Department, Mechatronics ∗ Section, AP 14-740, 07000 Ciudad de Mexico, Mexico (e-mail:
CINVESTAV, Electrical Engineering Mechatronics [email protected], Section, AP 14-740, 07000 [email protected]). deDepartment, Mexico, Mexico (e-mail: [email protected], Section, AP 14-740, 07000 [email protected]). de Mexico, Mexico (e-mail: [email protected], [email protected]). [email protected], [email protected]). Abstract: Abstract: In In this this paper paper we we propose propose aa non-linear non-linear control control law law to to achieve achieve trajectory trajectory tracking tracking in in the 3D space for a commercial AR.Drone 2.0 quadrotor. The proposed control law is based on Abstract: In this paper we propose a non-linear control law to achieve trajectory tracking in the 3D space for a commercial AR.Drone 2.0 quadrotor. The proposed control law is based on Abstract: Intime thisa varying paper we propose athe non-linear controltechnique law proposed to achieve trajectory tracking in a non-linear version of Backstepping that takes into account the the 3D space for commercial AR.Drone 2.0 quadrotor. The control law is based on athenon-linear time version of the Backstepping technique that control takes into account the 3D space for internal a varying commercial AR.Drone 2.0 quadrotor. The proposed law is based on dynamics of the orientation controller of the AR.Drone. The stability of the control a non-linear time varying version of the Backstepping technique that takes into account the dynamics of the internal orientation of the AR.Drone. The stability the control a non-linear time varying version of controller the Lyapunov Backstepping technique that takes intoof the law is ensured by the design of recursive functions and its is tested by dynamics of the internal orientation controller of the AR.Drone. Theperformance stability of account the control law is ensured by the design of recursive Lyapunov functions and its performance is tested by dynamics of the internal orientation controller of the AR.Drone. The stability of the control numeric simulations on MATLAb Simulink environment. law is ensured by the design of recursive Lyapunov functions and its performance is tested by numeric simulations on MATLAb Simulink environment. law is ensured by theondesign of recursive Lyapunov functions and its performance is tested by numeric simulations MATLAb Simulink environment. © 2018, IFAC (International FederationSimulink of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. numeric simulations on MATLAb environment. Keywords: control Keywords: Quadrotors, Quadrotors, Trajectory Trajectory tracking, tracking, Backstepping, Backstepping, non-linear non-linear control Keywords: Quadrotors, Trajectory tracking, Backstepping, non-linear control Keywords: Quadrotors, Trajectory tracking, Backstepping, non-linear control 1. INTRODUCTION well a dynamical model for this internal controller. We We 1. INTRODUCTION well a dynamical model this internal controller. base the modelingmodel of thefor internal controller on a paper 1. INTRODUCTION well a dynamical for this internal controller. We base the modeling of the internal controller on a paper 1. INTRODUCTION athe dynamical model this the internal controller. We by Bristeau et al. (2011) where AR.Drone base modeling of thefor internal controller on ainternal paper While many possible types of small Unmanned Aerial Ve- well by Bristeau et al. (2011) where the AR.Drone internal While many possible types of smallhelicopter Unmanned Aerial Ve- base the modeling of the internal controller on ainternal paper programming is presented, however most parameters are hicles (UAV) exist, the quadrotor have become by Bristeau et al. (2011) where the AR.Drone While many possible types of small Unmanned Aerial Veprogramming presented, however most parameters are hicles (UAV) exist, the quadrotor helicopter have become by Bristeau Specifically, etis al. (2011)according where the AR.Drone internal undisclosed. to said paper, we asWhile (UAV) many possible types of small Unmanned Aerial Vepopular in research applications because of its wide area of programming is presented, however most parameters are hicles exist, the quadrotor helicopter have become undisclosed. Specifically, according to said paper, we aspopular in research applications because of its wide area of programming is presented, however most parameters are sume the angular dynamics under the internal controller hicles (UAV) exist, the quadrotor helicopter have become undisclosed. Specifically, according to said paper, we aspossible in applications, as seen in because Gupte etofal. This popular research applications its (2012). wide area of sume the angular dynamics under the internal controller possible applications, as seen in Gupte et al. (2012). This undisclosed. Specifically, according to said paper, we asobey second order transfer functions. Then the parameters popular in research applications because of its wide area of sume the angular dynamics under the internal controller is due to its small size and weight, and its ability to move possible applications, as seen in Gupte et al. (2012). This obey second order transfer functions. Then the parameters is due to its smallinside sizeas and weight, and its ability toand move sume the angular dynamics under the internal controller of such transfer functions are identified through the least possible applications, seen indimensional Gupte et al. (2012). This in any direction a three space its obey second order transfer functions. Then the parameters is due to its small size and weight, and its ability to move such transfer functions are identified through the least in any direction inside a three dimensional spacetoand its of second order transfer functions. Then thedata. parameters squares method by the analysis of navigation is due its small size and weight, and ability move ability to hover and vertical take-off anditslanding. of such transfer functions are identified through the least in any to direction inside a three dimensional spaceRegardand its obey squares method by the analysis of navigation data. ability to hover and vertical take-off and landing. Regardof such transfer functions are identified through the least in any direction inside a three dimensional space and its squares method by the analysis of navigation data. less of its many and advantages one of the drawbacks ability to hover vertical take-off andbiggest landing. RegardWe have previous works about the trajectory tracking less ofquadrotor its many and advantages one of the biggest drawbacks squares method by the analysis of navigation data. We have previous works about the trajectory tracking ability to hover vertical take-off and landing. Regardof a is its reduced payload which also limits less of its many advantages one of the biggest drawbacks problem for one quadrotor, see Santiaguillo-Salinas et of a quadrotor is its reduced payload which also limits We have previous works about the trajectory tracking problem for one quadrotor, see Santiaguillo-Salinas et al. al. lessaamount ofquadrotor its many advantages one ofbethe biggest drawbacks the of batteries that can carried and its flight of is its reduced payload which also limits We haveand previous works about the see trajectory tracking (2017), a group of quadrotors, Rosaldo-Serrano the amount of batteries that can be carried and its flight problem for one quadrotor, see Santiaguillo-Salinas et al. and a group of quadrotors, see Rosaldo-Serrano of aamount quadrotor is its isreduced payload which also time. The quadrotor athat system with four propellers in (2017), the of batteries can be carried and its limits flight for one quadrotor, see Santiaguillo-Salinas et al. et al. said we based (2017), and aIn group of quadrotors, see controllers Rosaldo-Serrano time. The quadrotor is athat system with four and propellers in problem al. (2016). (2016). said works works we designed designed controllers based amount ofconfiguration. batteries can be carried athe cross shape While the front and its theflight rear time. The quadrotor is a system with four propellers in et (2017), and order aIn group of quadrotors, see of Rosaldo-Serrano on the first linear approximation the AR.Drone amotor cross shape configuration. While the front and the rear et al. (2016). In said works we designed controllers based the(2016). first order linear approximation of the AR.Drone quadrotor is the a system four propellers in on rotate clockwise, left andwith the front right motor rotate atime. crossThe shape configuration. While the and the rear et al. Inmain said works we designed controllers based dynamics. contribution of this paper is motor the left and the front right motor rotate the firstThe order linear approximation of the AR.Drone dynamics. The main contribution of this paper is the the dedea crossrotate shapeclockwise, configuration. While the and the rear on counter-clockwise which nearly cancels gyroscopic effects motor rotate clockwise, the left and the right motor rotate on the first order linear approximation ofpaper the AR.Drone sign of a nonlinear control strategy for trajectory tracking counter-clockwise which nearly cancels gyroscopic effects dynamics. The main contribution of this is the deof a nonlinear control strategy for trajectory tracking motor rotate clockwise, and the right motor rotate sign and aerodynamic torques in left trimmed flight. One additional counter-clockwise whichthe nearly cancels gyroscopic effects dynamics. The main contribution offorthis paper isthe thePardeof a quadrotor in the 3D space, specifically, for and aerodynamic torques in trimmed flight. One additional sign of a nonlinear control strategy trajectory tracking a of quadrotor in the 3D space, specifically, for the Parcounter-clockwise which nearly cancels gyroscopic effects of advantage of the torques quadrotor compared to aOne conventional and aerodynamic in trimmed flight. additional sign a nonlinear control strategy for trajectory tracking rot’s AR.Drone 2.0.the The proposed control strategy consist advantage of the quadrotor compared to a conventional of a quadrotor in 3D space, specifically, for the Parand aerodynamic in trimmed flight. additional AR.Drone in 2.0. The control strategy consist helicopter is simplified rotor mechanics. By varying rot’s advantage of the the torques quadrotor compared to aOne conventional of a quadrotor the 3Dproposed space, specifically, for the Parnon-linear time-varying version of the backstepping helicopter is the simplified rotor mechanics. By varying rot’s AR.Drone 2.0. The proposed control strategy consist advantage the quadrotor compared to aapplied conventional of a non-linear time-varying version of the backstepping the thrust of a single motor, the lift force to the helicopter is the simplified rotor mechanics. By varying rot’s AR.Drone 2.0. Thethe proposed control strategy consist technique. To validate theoretical results we present the thrust of a single motor, the lift force applied to the of a non-linear time-varying version of the backstepping To validate the theoretical we present helicopter is simplified mechanics. By varying quadrotor manipulated to produce and/or the thrust can of the abe single motor,rotor the lift force vertical applied to the technique. of a non-linear time-varying version ofresults the backstepping numerical simulations on the MATLab simulink quadrotor can be manipulated to produce vertical and/or technique. To validate the theoretical results weenvironpresent numerical simulations on the MATLab simulink the thrust of a single motor, the lift force applied to the lateral motion. In spite of the four the quadrotor quadrotor can be manipulated to actuators, produce vertical and/or technique. To validate thethe theoretical results weenvironpresent ment. The main advantage of the controller proposed in lateral motion. In spite of the four actuators, the quadrotor numerical simulations on MATLab simulink environThe simulations main advantage ofMATLab the controller proposed in quadrotor can be manipulated to with produce vertical and/or ment. is aa dynamically system nonlinear dynamics lateral motion. Inunstable spite of the four actuators, the quadrotor numerical on the simulink environthis paper is its easy implementation and proven stability, is dynamically unstable system with nonlinear dynamics ment. The main advantage of the controller proposed in lateral motion. In spite of the four actuators, the algorithm. quadrotor this paper is its easy implementation and proven stability, that has to be stabilized by a suitable control is a dynamically unstable system with nonlinear dynamics ment. Thethe advantage of Lyapunov the controller proposed in based on recursive use of functions, while that has to be stabilized by a suitable control algorithm. this paper ismain its easy implementation and proven stability, based on the recursive use of Lyapunov functions, while is a dynamically unstable system with nonlinear dynamics Manyhas of to thebeproposed for control quadrotors, as in this that stabilizedcontrollers by a suitable algorithm. paper isnon-linearities its easy implementation and proven stability, taking the of the dynamics directly into Many of the proposed controllers for quadrotors, as in based on the recursive use of Lyapunov functions, while thethe non-linearities the dynamics directlywhile into that has stabilized by a suitable control algorithm. Wang et al. (2016), arecontrollers based on aforlinearized model of taking Many of to thebe proposed quadrotors, as in on recursive useof Lyapunov functions, account. Wang et arecontrollers based on aforlinearized model of based taking the non-linearities ofof the dynamics directly into account. Many of al. the (2016), proposed quadrotors, as in the quadrotor. Wang et al. (2016), are based on a linearized model of taking the non-linearities of the dynamics directly into the quadrotor. Wang et al. (2016), are based on a linearized model of account. This the quadrotor. account. This paper paper is is organized organized as as follows. follows. Section Section 2 2 presents presents For many research groups it is common to use commercial the quadrotor. the nonlinear dynamical model of the AR.Drone 2.0. For many research groups it is common to use commercial This paper is organized as follows. Section 2 presents the nonlinear dynamical model of the AR.Drone 2.0. quadrotors due to their accessible prices and reliability. For many research groups it is common to use commercial This nonlinear paper isthe organized as follows. Section 2 presents obtained by combination of the classic model of quadrotors due to their accessible prices and reliability. the dynamical model of the AR.Drone 2.0.a obtained by the combination of the classic model of many research groups it decided is common to use commercial For this project, our group to use the AR.Drone quadrotors due to their accessible prices and reliability. the nonlinear dynamical model of the AR.Drone 2.0.a generic quadrotor and the identified internal controller For this project, our group decided to use the AR.Drone obtained by the combination of the classic model of a generic quadrotor and the identified internal controller quadrotors due to their accessible prices and reliability. 2.0 quadrotor manufactured by Parrot. As seen in Krajnk obtained For this project, our group decided to use the AR.Drone by the combination of3 the classic model of of a of the AR.Drone 2.0 . Section presents the design 2.0 quadrotor manufactured by Parrot. As seen in Krajnk generic quadrotor and the identified internal controller the AR.Drone 2.0 . Section 3 presents the controller design of Foral. this project, group decided to use the AR.Drone et (2011), theour AR.Drone a popular choice among of 2.0 quadrotor manufactured byis Parrot. As seen in Krajnk generic quadrotor and the identified internal the nonlinear control strategy for 3D trajectory tracking. et (2011), the AR.Drone a popular choice among the AR.Drone 2.0 strategy . Sectionfor3 3D presents the design of the nonlinear control trajectory tracking. 2.0 al. quadrotor manufactured byis Parrot. Aslarge seen in Krajnk researchers because of its low and number of of et al. (2011), the AR.Drone iscost a popular choice among of the AR.Drone 2.0numeric . Section 3 3D presents the design of Section 44 presents simulations to validate the researchers because of its low cost and large number of the nonlinear control strategy for trajectory tracking. Section presents numeric simulations to validate the et al. (2011), the The AR.Drone a2.0, popular choice among sensors on board. as many other comresearchers because of AR.Drone its lowiscost and large number of the nonlinear control strategy for 3D trajectory tracking. designed control strategy. Finally, in Section 5 we present Section 4 presents numeric simulations to validate the sensors on board. The AR.Drone 2.0, as many other comstrategy. Finally, in Section we present researchers because of AR.Drone itsanlow cost large other number of designed mercial quadrotors, have internal stabilizing controller sensors on board. The 2.0,and as many comSection 4control presents numeric simulations to 55validate the conclusions and possible future work. mercial quadrotors, have an internal stabilizing controller designed control strategy. Finally, in Section we present conclusions and possible future work. sensors on board. The AR.Drone 2.0, as many other comfor its Euler angles. For the correct implementation of mercial quadrotors, have an internal stabilizing controller designed control strategy. Finally, in Section 5 we present for its Euler angles.have For an theinternal correct implementation of conclusions and possible future work. mercial quadrotors, stabilizing controller our proposed trajectory controller we for its Euler angles. Fortracking the correct implementation of conclusions and possible future work. our proposed trajectory tracking controller we propose propose as as for its Euler angles. For the correct implementation of our proposed trajectory tracking controller we propose as our proposed trajectory tracking controller we propose as This work was financial supported by CONACyT, Mexico, through
This work was financial supported by CONACyT, Mexico, through theThis scholarship work was280703. financial supported by CONACyT, Mexico, through the scholarship 280703. This work was280703. financial supported by CONACyT, Mexico, through the scholarship the scholarship 280703. 2405-8963 © 2018, IFACConference (International Proceedings, 2nd IFAC onFederation of Automatic Control) 532 Hosting by Elsevier Ltd. All rights reserved. Proceedings, 2nd IFAC Conference on 532 Control. Peer reviewIdentification under responsibility of International Federation of Automatic Modelling, and Control of Nonlinear Proceedings, 2nd IFAC Conference on 532 Modelling, Identification and Control of Nonlinear 10.1016/j.ifacol.2018.07.334 Systems Proceedings, 2nd IFAC Conference on 532 Modelling, Systems Identification and Control of Nonlinear Guadalajara, Mexico, June 20-22, 2018 Modelling, Identification and Control of Nonlinear
2018 IFAC MICNON Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
533
Using the above equations, we can represent the dynamical model of a generic quad-rotor helicopter as u x ¨=− (cos ψ sin θ cos φ + sin ψ sin φ) (4a) m u y¨ = − (sin ψ sin θ cos φ − cos ψ sin φ) (4b) m u z¨ = − (cos θ cos φ) + g (4c) m (4d) ψ¨ =˜ τψ ¨ (4e) θ =˜ τθ (4f) φ¨ =˜ τφ τθ , where the total thrust u and the angular momenta τ˜ψ ,˜ and τ˜φ are the control inputs of the system. 2.2 Modeling of the AR.Drone 2.0 internal controller Fig. 1. Quad-rotor helicopter scheme. 2. DYNAMICAL MODEL OF THE AR.DRONE 2.0 2.1 Dynamical Model for a generic quadrotor Consider a quadrotor helicopter moving in a three dimensional space. We define a North-East-Down (NED) inertial frame Fe fixed to earth and a xyz body frame Fb fixed to the center of mass of the quadrotor helicopter, as shown in Fig. 1. The position and orientation of the body frame Fb with respect to the fixed frame Fe are given by T T ξ = [x, y, z] and η = [ψ, θ, φ] , respectively. Where x,y and z are the spatial coordinates of the quadrotor center of mass and ψ, θ, and φ are the respective Euler angles of yaw, pitch and roll of the quadrotor body frame. Based on Garca-Carrillo et al. (2013), the nonlinear dynamics of the quad-rotor helicopter can be expressed as mξ¨ = − mgD + RF (1) ˙ = − Ω × IΩ + τ IΩ
(2)
T
where D = [0, 0, −1] , R ∈ SO(3) is a rotation matrix 4 T relating Fe with Fb , F = [0, 0, u] with u = i=1 Fi is the total thrust applied to the quadrotor by the four rotors, m is the total mass of the quadrotor, g is the standard acceleration due to gravity, I is the inertia matrix of the quadrotor in body frame, and τ is the total torque applied to the quadrotor by the four rotors. The variable Ω = T [r, q, p] represents the angular velocity of the quadrotor defined by Ω = W η˙ − sin θ 0 1 W = cos θ sin φ cos φ 0 . cos θ cos φ − sin φ 0 Demoting cos(∗) and sin(∗) as c∗ and s∗ , respectively, the rotation matrix R ∈ SO(3) is given by c θ cψ sφ sθ cψ − cφ sψ cφ sθ cψ + sφ sψ R = cθ sψ sφ sθ sψ + cφ cψ cφ sθ sψ − sφ cψ −sθ sφ cθ cφ cθ We define an auxiliary vector τ˜ related to generalized torque τ and based on eq. (2) as τ˜ψ ˙ η˙ − W η˙ × IW η˙ + τ ) (3) τ˜ = τ˜θ = I −1 W −1 (−I W τ˜φ 533
In Bristeau et al. (2011) the authors show the main lines of the internal controller embedded in the AR.Drone 2.0, which stabilizes the angular dynamics. However, the actual parameters of the internal controller remain undisclosed. We assume that the dynamics of the internal controller of the AR.Drone can me modeled by the following differential equations (5a) z¨ = − a1 z˙ + a3 uz ˙ ¨ (5b) φ = − b1 φ − b2 φ + b3 uφ θ¨ = − c1 θ˙ − c2 θ + c3 uθ ψ¨ = − d1 ψ˙ + d3 uψ
(5c)
(5d) where the control inputs of the AR.Drone are the desired roll and pitch angles uφ and uθ , the desired ascending velocity uz and the desired rotational velocity for yaw uψ . The parameters ai , bi , ci and di were calculated by analyzing the data obtained from several experiments performed with step inputs. The data obtained was processed with the MATLab software using the least square method from its system identification toolbox. Is noteworthy that the AR.Drone doesn’t allow the direct control of the total thrust u. To obtain the complete non-linear model for the AR.Drone we compute the total thrust u from (4c) and substitute it in (4a) and (4b). Then we add the equations for the internal controller of the AR.Drone we obtained in (5a)-(5d). Finally, we obtain tan φ sin ψ (6a) x ¨ =(−a1 z˙ + a3 uz − g) tan θ cos ψ + cos θ tan φ cos ψ (6b) y¨ =(−a1 z˙ + a3 uz − g) tan θ sin ψ − cos θ z¨ = − a1 z˙ + a3 uz (6c) ˙ ¨ (6d) φ = − b1 φ − b2 φ + b3 uφ θ¨ = − c1 θ˙ − c2 θ + c3 uθ ψ¨ = − d1 ψ˙ + d3 uψ
(6e)
(6f) Notice that the control inputs uz , uθ , uφ and uψ for the complete system are the same as in the internal controller of the AR.Drone. 3. NON-LINEAR CONTROL DESIGN 3.1 Altitude and yaw orientation control Our purpose is to design a control algorithm to achieve trajectory tracking for a quadrotor in 3D space. First we
2018 IFAC MICNON 534 Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
consider that the dynamics of the altitude and yaw angle of our quadrotor, given by (6c) and (6f), are decoupled for the rest of the (6a)-(6f) system and propose a feedforward PD controller to control them. Define the states for the altitude and yaw dynamics, respectively, by z1 =z (7a) (7b) z2 =z˙ ψ1 =ψ (7c) (7d) ψ2 =ψ˙ and define the tracking errors by ez1 =z1 − z1d (8a) ez2 =z2 − z˙1d
(8b)
eψ2 =ψ2 −
(8d)
eψ1 =ψ1 − z1d (t)
ψ1d ψ˙ 1d
(8c)
where is the desired trajectory for the altitude z1 and ψ1d (t) is the desired trajectory for the yaw angle ψ1 . For the control of the altitude and yaw dynamics we propose the PD controllers given by 1 a1 z2 − kpz ez1 − kdz ez2 + z¨1d (9) uz = a3 1 d1 ψ2 − kpψ eψ1 − kdψ eψ2 + ψ¨1d (10) uψ = d3 where a1,3 and d1,3 are parameters we obtained while modeling the AR.Drone internal dynamics for the altitude and yaw orientation and kpz , kdz , kpψ and kdψ are the gains of the controllers. Since the altitude and yaw dynamics are decoupled from the rest of the system, it follows straightforwardly that the states z1 and ψ1 converge exponentially to their desired trajectories z1d and ψ1d , respectively.
ξ˙2 =gz (t)A(ψ1 )η1∗ η˙ 1∗ =η2∗ η˙ ∗ =M˙ η1 η2 + Mη1 (−β1 η2 − β2 η1 + β3 uη ) 2
For the design of the controller for the quadrotor motion in the XY plane we propose a time-varying non-linear backstepping controller based on Kokotovic (1992) and Khalil (2002). Define the states for the X, Y , θ and φ dynamics as ξ1 = [x, y]
T
(11a)
ξ2 = [x, ˙ y] ˙
T
(11b)
T
(11c) (11d)
Using these states we can rewrite the nonlinear system (6a),(6b),(6d),(6e) as ξ˙1 =ξ2 (12a) tan η 11 ξ˙2 =gz (t)A(ψ1 ) (12b) tan η12 / cos η11 η˙ 1 =η2 (12c) η˙ 2 = − β1 η2 − β2 η1 + β3 u (12d) ci 0 where βi = for i = 1, 2, 3, are constant matrices 0 bi corresponding to the parameters obtained in the modeling T of the AR.Drone internal controller, u = [uθ , uφ ] are 534
(13b) (13c) (13d)
Using the auxiliary controller ˙ η1 η2 + vη ) uη = β3−1 β1 η2 + β2 η1 + Mη−1 ( M 1 the system (13a)-(13d) is reduced to ξ˙1 =ξ2
ξ˙2 =gz (t)A(ψ1 )η1∗ η˙ 1∗ =η2∗ η˙ 2∗ =vη Define now the system errors as eξ1 =ξ1 − ξ1d eξ2 =ξ2 − ξ˙d 1
eη1 =η1∗ − η1∗d eη2 =η2∗
ξ1d
3.2 Position control in the XY plane
η1 = [θ, φ] T ˙ φ˙ η2 = θ,
the control inputs for the system, and gz (t) = −a1 z2 + cos ψ1 sin ψ1 a3 uz − g and A(ψ1 ) = are considered as sin ψ1 − cos ψ1 time-varying parameters. We apply a coordinate change to (11a)-(11d), tan η11 , η1∗ = tan η12 / cos η11 η2∗ = Mη1 η2 were sec2 η11 0 Mη1 = sec η11 tan η11 tan η12 sec η11 sec2 η12 and obtain the new system (13a) ξ˙1 =ξ2
−
η˙ 1∗d
(14a) (14b) (14c) (14d) (15a) (15b) (15c) (15d)
where is the desired trajectory of the agent in the XY plane and η1∗d is a function of the desired acceleration ξ¨1d given by d tan η11 η1∗d = d d / cos η11 tan η12 η1d = tan−1 (gz−1 (t)Aθψ (ψ1 )ξ¨1d ) cos ψ sin ψ with Aθψ = . The dynamics d d cos η11 sin ψ − cos η11 cos ψ of the system in error coordinates become (16a) e˙ ξ1 =eξ2 e˙ ξ2 =gz (t)A(ψ1 )eη1 (16b) (16c) e˙ η1 =eη2 e˙ η2 =vη − η¨1∗d (16d) Rewriting the error dynamics in matrix form and defining vη = η¨1∗d + wη we have e˙ ξ =Aξ eξ + Bξ (t)eη1 (17a) e˙ η1 =eη2 (17b) (17c) e˙ η2 =wη T
where wis thecontrol input of the system, eξ = [eξ1 , eξ2 ] , 0 I2 Aξ = is a constant matrix with I2 ∈ R2×2 0 0 0 the identity matrix and Bξ (t) = gz (t) is a A(ψ1 )
2018 IFAC MICNON Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
time-varying matrix depenfing on the known time-varying funtions gz and A(ψ1 ). We can note the system (17a)(17c) is in strict feedback form, meaning that we can use the backstepping algorithm to stabilize it. To begin the backstepping design process, we need a control law able to stabilize the subsystem (17a). Considering eη1 as the virtual input of the subsystem we propose the non-linear control law eη1 = α1 (eξ , t) given by tan−1 (γ1 ) α1θ = (18) α1 (eξ , t) = α1φ tan−1 (γ2 cos α1θ ) where γ1 = gz−1 A(ψ1 )−1 [−kp eξ1 − kd eξ2 ] γ2
were kp , kd > 0 are the controller gains. Is noteworthy that the control law (18) is bounded. The closed-loop system (17a)-(18) produces (19) e˙ ξ = Heξ with 0 1 ⊗ I2 H= −kp −kd where ⊗ denotes the Kronecker product. Now we propose the following Lyapunov function candidate 1 (20) V1 = eTξ P eξ 2 where the matrix P has the form a b (21) P = ⊗ I2 . b c taking a = 1 + kp−1 kd−1 kp + kd kp−1 , b = kp−1 and c = 1 + kp−1 kd−1 . It follows that 1 1 (22) V˙ 1 = eTξ P H + H T P eξ = − eTξ Qeξ < 0 2 2 for all eξ = 0. Therefore the closed-loop system (17a) with the virtual control input (18) is exponentially stable. Now that we have a known stabilizing control law and Lyapunov function for the subsystem (17a) we can start the backstepping procedure. First, take the subsystem (17a)-(17b) ˜ξ eη2 e˜˙ ξ = A˜ξ (t)˜ eξ + B (23) eξ A B (t) ξ ξ ˜ξ = 0 . , A˜ξ (t) = and B where e˜ξ = I2 0 0 eη1 Taking eη2 as a virtual input, we expand the stabilizing properties of (18) to (23) by the backstepping process. For this first step of backstepping, we define a virtual state y2 given by the difference between the real state eη1 and the virtual control law α1 y2 = eη1 − α1 (eξ , t) (24) y˙ 2 = eη2 − α˙ 1 (eξ , t) (25) changing the subsystem to the coordinates of y2 and using the virtual control law eη2 = α˙ 1 (eξ , t) + u1 we get (26a) e˙ ξ = [Aξ eξ + Bξ (t)α1 (eξ , t)] + Bξ (t)y2 y˙ 2 =u1 (26b) Consider the Lyapunov function candidate 1 (27) V2 = V1 + y2T y2 2 and its derivative 1 (28) V˙ 2 ≤ − eTξ Qeξ + y2T [BξT (t)P eξ ] + y2T u1 2 535
535
we choose the control law (29) u1 = −[BξT (t)P eξ ] − k1 y2 with k1 > 0 as the control gain, to obtain 1 (30) V˙ 2 ≤ − eTξ Qeξ − y2T k1 y2 2 Therefore the subsystem (26a)-(26b)-(29) is exponentially stable. Substituting u1 , α˙ 1 (eξ , t) and y2 in the virtual control eη2 = α2 (eξ , eη1 , t) we have α2 =α˙ 1 − BξT (t)P eξ − k1 (eη1 − α1 (eξ ))
(31)
Finally we expand the stabilizing properties of (31) to the control law w for the complete system (16a)-(16d). For this second step of backstepping, we define the virtual variable y3 as y3 = eη2 − α2 (eξ , eη1 , t) (32) (33) y˙ 3 = w − α˙ 2 (eξ , eη1 , t) changing the system to the coordinates of y3 and using the virtual control law w = α˙ 2 (eξ , eη1 , t) + u2 we get ˜ξ α2 (eξ , eη1 , t) + B ˜ ξ y3 e˜˙ ξ = A˜ξ (t)˜ eξ + B (34a)
y˙ 3 =u2 (34b) Consider the Lyapunov function candidate 1 (35) V3 = V2 + y3T y3 2 and its derivative 1 (36) V˙ 3 ≤ − eTξ Qeξ − y2T k1 y2 + y3T y2 + y3T u2 2 we choose the control law (37) u2 = −y2 − k2 y3 with k2 > 0 as the control gain, to obtain 1 (38) V˙ 3 ≤ − eTξ Qeξ − y2T k1 y2 − y3T k2 y3 2 so that the subsystem (34a)-(34b)-(37) is exponentially stable. Substituting u2 , α˙ 2 (eξ , eη1 , t) and y3 in the control law w we obtain the control law wη =α˙ 2 (eξ , eη1 ) − (eη1 − α1 (eξ )) − k2 (eη2 − α2 (eξ , eη1 )) (39) for the motion control of the quadrotor in the XY plane. 4. NUMERICAL SIMULATIONS
To validate the designed control law (39) we perform numerical simulations on MATLab Simulink environment. The dynamical model of the AR.Drone 2.0 was programmed using the parameters obtained from the experimental analysis we did for the modeling of its internal controller. We consider that all the states are known and can be tracked at all times. The desired trajectory in the XY plane is defined as a Lemniscate of Gerono given by xd = sin(wt) y d = sin(wt) cos(wt) while the desired altitude and yaw trajectories are defined by z d = 1 + 0.2 sin(wt) ψ d = sin(wt) with w = 2π 60 for all the trajectories. Figure 2 shows the trajectory tracking by the quadrotor in the 3D space.
2018 IFAC MICNON 536 Guadalajara, Mexico, June 20-22, 2018M.A. Rosaldo-Serrano et al. / IFAC PapersOnLine 51-13 (2018) 532–536
Figures 3 and 4 show the respective position errors in X, Y and Z, and the orientation errors in θ, φ and ψ. The error graphics were trimmed to 10 seconds to be able see the errors with more detail at the transitory time, even when the desired trajectory needs 60 seconds to complete a full loop. We can note all of the errors converge exponentially to 0 in a short time. Trajectory control
the control law was proven by the design of adequate Lyapunov functions and its performance was tested by numeric simulations on MATLab Simulink software. As future work we propose the real-time implementation of the designed control law on an AR.Drone 2.0, the design of non-linear observers for the estimation of its linear and angular velocities. Also we consider the possibility to expand the designed control law for the control of a multiagent system with quadrotors as the system agents.
1.5
z position (m)
REFERENCES 1
Bristeau, P.J., Callou, F., Vissire, D., and Petit, N. (2011). The navigation and control technology inside the ar.drone micro uav. 18th IFAC World Congress, Milano, Italy, 1477–1484. Garca-Carrillo, L.R., Dzul-Lpez, A.E., Lozano, R., and Pgard, C. (2013). Modeling the Quad-Rotor MiniRotorcraft. Springer, London. Gupte, S., Mohandas, P., and Conrad, J. (2012). A survey of quadrotor uavs. 2012 Proceedings of IEEE Southeastcon, Orlando, FL, USA. Khalil, H.K. (2002). Nonlinear Systems. Prentice Hall. Kokotovic, P.V. (1992). The joy of feedback: nonlinear and adaptive. IEEE Control Systems Magazine, 12, 7–17. Krajnk, T., Vonsek, V., Fiser, D., and Faigl, J. (2011). Ardrone as a platform for robotic research and education. International Conference, Prague, Czech Republic, 172– 186. Rosaldo-Serrano, M.A., Santiaguillo-Salinas, J., and Aranda-Bricaire, E. (2016). Observer-based timevarying backstepping control for a quadrotor multiagent system. 2017 International Conference on Unmanned Aircraft Systems ICUAS’17, Miami, FL, USA, 1324–1332. Santiaguillo-Salinas, J., Rosaldo-Serrano, M.A., and Aranda-Bricaire, E. (2017). Observer-based timevarying backstepping control for parrots ar.drone 2.0. 20th IFAC World COngress, Toulouse, France, 10794– 10799. Wang, P., Man, Z., Cao, Z., Zheng, J., and Zhao, Y. (2016). Dynamics modelling and linear control of quadcopter. International Conference on Advanced Mechatronic Systems, Melbourne, VIC, Australia.
0.5
0 1 0.5 0 −0.5
y position (m)
0
−0.5
−1
Desired trajectory 0.5 Real trajectory
1
x position (m)
Fig. 2. Desired and actual trajectories Error (m)
Position Error (x) 0.2 0 −0.2
0
1
2
3
4
5
6
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
7
8
9
10
Error (m)
Time (s) Position Error (y) 0.2 0 −0.2
0
1
2
3
4
5
6
Error (m)
Time (s) Position Error (z) 2 0 −2
0
1
2
3
4
5
6
Time (s)
Error (rad)
Error (rad)
Error (rad)
Fig. 3. Position Errors Orientation Error (θ) 0.05 0 −0.05
0
1
2
3
4
5
6
Time (s) Orientation Error (φ) 0.01 0 −0.01
0
1
2
3
4
5
6
Time (s) orientation Error (ψ) 0.05 0 −0.05
0
1
2
3
4
5
6
Time (s)
Fig. 4. Orientation Errors 5. CONCLUSIONS AND FUTURE WORK We successfully designed a non-linear control law able to achieve trajectory tracking in the 3D space for a commercial AR.Drone 2.0 quadrotor. The stability of 536