Control of a Quadrotor Mini-Helicopter via Full State Backstepping Technique

Transcription

1 Proceedings of the 45th IEEE Conference on Decision & Control Manchester Grand Hyatt Hotel San Diego, CA, USA, December 3-5, 006 Control of a Quadrotor Mini-Helicopter via Full State Backstepping Technique Tarek Madani and Abdelaziz Benallegue Abstract In this paper, we present a new control approach for a quadrotor mini-helicopter using the full state backstepping technique. The controller can set the helicopter track three Cartesian position and the yaw angle to their desired trajectories and stabilize the pitch and roll angles by varying the input signals of DC-motors. The quadrotor has been presented into three interconnected subsystems. The first one representing the under-actuated subsystem, gives the dynamic relation of the horizontal positions with the pitch and roll angles. The second fully-actuated subsystem represents the dynamic behavior of the vertical position and the yaw angle. The last subsystem gives the dynamic of the four rotors propeller system. The design controller methodology is based on the Lyapunov stability theory. Various simulations of a quadrotor show the good performance of the proposed control law. Finally, we present initial flight experiments where the mini-helicopter is restricted to vertical and yaw motions. I. INTRODUCTION The helicopter are being used more and more in civilian applications such as traffic monitoring, recognition and surveillance vehicles, search and rescue operations. There are several types of these machines. The classical one is conventionally equipped with a main rotor and tail rotor. Other types exist, including the twin rotor or tandem helicopter and the coaxial rotor helicopter. The quadrotor helicopter, like the one shown in figure, has some advantages over conventional helicopters. It has four rotors which turn at angular velocities ω, ω, ω 3 and ω 4. The two pairs of rotors (, 3) and (, 4) turn in opposite direction in order to balance the moments and produce yaw motions as needed. On varying the rotor speeds altogether with the same quantity the lift forces will change affecting in this case the altitude z of the system. Yaw angle ψ is obtained by speeding up the clockwise motors or slowing down depending on the desired angle direction. The motion direction according (x, y) axes depends on the sense of tilt angles (pitch angle φ and roll angle θ) whether they are positive or negative. The model dynamic of the quadrotor helicopter has six outputs x, y, z, φ, θ, ψ} while it has only four independent inputs. Therefore the quadrotor is an under-actuated system. It is not possible to control all of the states at the same time. A possible combination of controlled outputs can be x, y, z, ψ} in order to track the desired position trajectory, more to an arbitrary heading and stabilize the other two angles, which introduces stable zero dynamics into the system T. Madani and A. Benallegue are with the Systems Engineering Laboratory of Versailles, 0-, avenue de l Europe, 7840 Vélizy - FRANCE. madani@lisv.uvsq.fr and benalleg@lisv.uvsq.fr Fig.. Quadrotor mini-helicopter [. A good controller should be able to reach a desired Cartesian position and a desired yaw angle while keeping the stabilization of the pitch and roll angles. The automatic control of a quadrotor helicopter has attracted the attention of many researches in the past few years [-[8. Generally, the control strategies are based on simplified models which have both a minimum number of states and minimum number of inputs. These reduced models should retain the main features that must be considered when designing control laws for real aerial vehicles. The control of the x and y motion present a challenging problem. Indeed, in order to control them, tilt angles (pitch and roll) need to be controlled. It appeared judicious for much researcher to apply the backstepping control technique to solve this problem [[6[. Bouabdallah and Siegwart proposed in [6 the backstepping controller using simplified model of the quadrotor and special decomposition of the control law (translation and rotation). The interest of the present work is to adapt our backstepping controller given in [, to control a mini-helicopter having four rotors driven by DC-motors. We are interested in the design of a controller to perform hover and tracking of desired trajectories. A control law strategy is proposed having in mind that the quadrotor can be seen as three interconnected subsystems: under-actuated subsystem (x and y outputs), fully-actuated subsystem (ψ and z outputs) and rotors subsystem (ω i outputs). The stabilization idea of the full closed loop system can be summarized as follows: The positions (x, y) are controlled by a virtual input /06/$ IEEE. 55

2 45th IEEE CDC, San Diego, USA, Dec. 3-5, 006 based on the tilt angles (φ, θ). The tilt angles (φ, θ) and the (ψ, z) motions are controlled by varying the rotor speeds (ω,...,ω 4 ), thereby changing the slope, the rotation and the lift forces. The paper is organized as follows: in section II, a mechanical and electrical dynamic model for a mini-helicopter is developed. We design in section III, a full state backstepping control law based on the nonlinear model. Simulations are carried out, in section IV, to show the performance and stability of the proposed controller. An experimental implementation of the proposed controller on the real quadrotor is shown in section V. Finally, section VI is a conclusion. II. DYNAMIC MODELING OF A QUADROTOR MINI-HELICOPTER The equations describing the attitude and position of a quadrotor helicopter are basically those of a rotating rigid body with six degrees of freedom [9 [0. The absolute position is described by ζ =[x, y, z T and its attitude by the three Euler s angles η =[φ, θ, ψ T. These three angles are respectively called pitch angle ( π <φ< π ), roll angle ( π <θ< π ) and yaw angle ( π ψ<π). The quadrotor is restricted with the six degrees of freedom according to the reference frame E m : Three translation velocities V = [V,V,V 3 T and three rotation velocities Ω=[Ω, Ω, Ω 3 T. The relation existing between the velocities vectors (V,Ω) and ( ζ, η) are: ζ = Rt V Ω=R r η () where R t and R r are respectively the transformation velocity matrix and the rotation velocity matrix between E a and E m such as: C φ C ψ S φ S θ C ψ C φ S ψ C φ S θ C ψ + S φ S ψ R t = C θ S ψ S φ S θ S ψ + C φ C ψ C φ S θ S ψ S φ C ψ S φ S φ C θ C φ C θ and 0 S θ R r = 0 C φ C θ S φ 0 S φ C φ C θ where S (.) and C (.) are the respective abbreviations of sin(.) and cos(.). One can write Ṙt = R t S(Ω) where S(Ω) denotes the skew-symmetric matrix such that S(Ω)v =Ω v for the vector cross-product and any vector v R 3. In other words, for a given vector Ω,the skew-symmetric matrix S(Ω) is defined as follows: S(Ω) = 0 Ω 3 Ω Ω 3 0 Ω () Ω Ω 0 The derivation of () with respect to time gives ζ = Rt V + Ṙ t V = R t V + Rt S(Ω)V = R t ( V +Ω V ) Ω =R r η + ( Rr φ φ + Rr θ θ ) η (3) Using the Newton s laws in the reference frame E m, about the quadrotor helicopter subjected to forces F ext and moments T ext applied to the epicenter, one obtains the dynamic equation motions: Fext = m V +Ω (mv ) (4) Text = I T Ω+Ω (IT Ω) where m and I T = diag[i x,i y,i z are respectively the mass and the total inertia matrix of helicopter, F ext and Text includes the external forces/torques developed in the epicenter of a quadrotor according to the direction of the reference frame E m, such as: Fext = F F aero F grav (5) Text = T T aero T gyro where the forces F, F aero,f grav } and the torques T,T aero,t gyro } are explained in the table I where G = [0, 0,g T is the gravity vector (g = 9.8m.s ), K t,k r } are two diagonal aerodynamic matrices, W i = [0, 0, ( ) i+ ω i T is the rotational velocity vector of the i-th rotor where ω i > 0 is the module of the rotation speed and I R is the inertia of the rotor. Model Source F =[0, 0,F 3 T T =[T,T,T 3 T propeller system F aero = K tv aerodynamic friction T aero = K rω T gyro = P 4 i= Ω I RW i gyroscopic effect F grav = mrt T G gravity effect TABLE I MAIN PHYSICAL EFFECTS ACTING ON A QUADROTOR The forces F and torques T produced by the propoller system of a quadrotor are: 0 dc t (ω ω 4) F = 0,T = dc t (ω3 ω) 4 (6) 4 c t i= ω i c d i= ( )i+ ωi where d is the distance from the epicenter of a quadrotor to the rotor axes, c t > 0 is the thrust factor and c d > 0 is the drag factor. Using (3), (4) and (5) allows to give the equation of the dynamics of rotation of the quadrotor expressed in the reference frame E a : F = mr T ζ t + K t R( t T ζ + mrt T G T = I T R r η + I Rr T φ φ + Rr θ ) φ η + K r R r η +(R r η) (I T R r η + (7) 4 i= I RW i ) The quadrotor are driven by four identical DC-motors with the well known equations [6: L di dt = u Ri k eω dω I R dt = k mi k r ω (8) k s where u =[u,u,u 3,u 4 T and ω =[ω,ω,ω 3,ω 4 T are respectively the inputs and the rotation speed modules of The vector ω =[ω,ω,ω 3,ω 4 T 56

3 45th IEEE CDC, San Diego, USA, Dec. 3-5, 006 rotors, i =[i,i,i 3,i 4 T the electrical courant of motors, L the electrical inductance, R the electrical resistance, k e > 0 the back EMF constant, k m > 0 the torque motor constant, k r > 0 the propeller aerodynamic resistant constant and k s > 0 the solid friction constant. The used motors are a very low inductance. The dynamic model (8) can be approximated by ω = a 0 a ω a ω + bu (9) where a 0 = ks I R, a = kmke I R R, a = kr I R and b = km I R R. III. BACKSTEPPING CONTROL OF A QUADROTOR In this section, we will design a new control law able to generate the input signals u =[u,u,u 3,u 4 T for DCmotors of a quadrotor helicopter ensuring that the position x(t),y(t),z(t),ψ(t)} tracks asymptotically the desired trajectory x d (t),y d (t),z d (t),ψ d (t)}. The dynamic model of a quadrotor is written in appropriate form suited for our control law. It is divided into three subsystems: an underactuated subsystem S, a fully-actuated subsystem S and rotors subsystem S 3. The control law of the whole system is computed by using the same steps of our work in [. However, the last step design has been modified to include a dynamic of the four rotors, which is used to generate the lift forces. The dynamic model for a quadrotor helicopter, developed in the previous section, can be rewritten in a state-space form by using the following state vectors: [ [ [ x φ ψ x =,x y 3 =,x θ 5 = z [ [,x 7 = ẋ ψ x =,x ẏ 4 =,x 6 = ż [ φ θ ω ω ω 3 ω 4 We obtain the stat-space equations of the three subsystems S,S and S 3 : ẋ = x ẋ S : = f 0 (x,x 3,x 5,x 6 )+g 0 (x 5,x 7 )ϕ 0 (x 3 ) ẋ 3 = x 4 ẋ 4 = f (x 3,x 4,x 6,x 7 )+g (x 3 )ϕ (x 7 ) ẋ5 = x S : 6 ẋ 6 = f (x 3,x 4,x 6,x 7 )+g (x 3 )ϕ (x 7 ) S 3 : ẋ 7 = f 3 (x 7 )+g 3 u (0) where the matrices g i (i =0,,, 3) are [ P 4 g 0 = ct i= ω i Sψ C ψ m, g C ψ S = ψ [ g = I z C φ Se θ 0 0 m C φc θ [ I x I y S φ T θ 0 I y C φ, g 3 = b () the vectors ϕ i (i =0,, ) are [ [ S ϕ 0 = φ dct (ω, ϕ C φ S = ω4) θ dc t (ω3 ω [ ) 4 cd ϕ = i= ( )i+ ωi () 4 c t i= ω i T θ and Se (.) are the abbreviations of tan(.) and cos(.) respectively and the vectors f i (i =0,,, 3) are [ [ fx fφ f 0 =, f f =, f y f = θ a 0 a ω a ω f 3 = a 0 a ω a ω a 0 a ω 3 a ω3 a 0 a ω 4 a ω4 with f x f y f z [ fψ f z, (3) = m R tk t Rt T ζ G (4) f φ f θ = (I T R r ) [I T ( Rr φ φ + Rr θ) η φ + K r R r η f ψ +(R r η) (I T R r η + 4 I R W i ) i= c d 4 Iz C φ T θ i= ( )i+ ωi + c d 4 Iz S φ i= ( )i+ ωi dc t I y S φ Se θ (ω3 ω) Using the backstepping technique, we can guarantee the convergence of the states x and x 5 of a quadrotor to follow the desired trajectory x d (t) =[x d (t),y d (t) T and x 5d (t) = [ψ d (t),z d (t) T. In this purpose, the control law of the whole system is computed by using the same design methodology of our work in [. However, the last step design has been modified to include the control of the four rotors, which is used to generate the lift forces. It is shown in [ that the stabilization of the two subsystems S and S can be obtained by using the following virtual controls: v = A z +ẋ d v S : = g0 (z + A z + v f 0 ) v 3 = J0 (gt 0 z + A 3 z 3 + v ) v 4 = g (J 0 T (5) z 3 + A 4 z 4 + v 3 f ) v5 = A S : 5 z 5 +ẋ 5d v 6 = g (z 5 + A 6 z 6 + v 5 f ) where A i R (i =,...,6) are a positive definite matrices, J 0 is the Jacobian matrix of ϕ 0 and z = x d x z S : = v x z 3 = v ϕ 0 (x 3 ) (6) z 4 = v 3 x 4 z5 = x S : 5d x 5 z 6 = v 5 x 6 By using (0), (5) and (6) we obtain the following form of the time derivative of z, z, z 3 and z 5 : ż = A z + z ż = z A z + g 0 z 3 ż 3 = g0 T (7) z A 3 z 3 + J 0 z 4 ż 5 = A 5 z 5 + z 6 The following part we will be devoted to stabilized the whole system S, S and S 3. Starting us by considering the 57

4 45th IEEE CDC, San Diego, USA, Dec. 3-5, 006 rotors subsystem S 3 : Let [ v4 ϕ z 7 = (x 7 ) v 6 ϕ (x 7 ) = ẋ 7 = f 3 (x 7 )+g 3 u (8) g (J 0 T z 3 + α 4 z 4 + v 3 f g ϕ }} ) ż 4 g (z 5 + α 6 z 6 + v 5 f g ϕ }} ) ż 6 (9) hence ż4 = J0 T z 3 A 4 z 4 + gz 7 ż 6 = z 5 A 6 z 6 + gz 7 (0) where g =[g, 0 and g =[0,g with 0 is a null matrix in R. The global Lyapunov function candidate of the whole quadrotor is V = 7 i= zt i z i () Its time derivative is given V = 7 i= zt i żi () = z T ( A z + z ) +z T ( z A z + g 0 z 3 ) +z3 T ( g0 T z A 3 z 3 + J 0 z 4 ) +z4 T ( J0 T z 3 A 4 z 4 + gz 7 ) +z5 T ( A 5 z 5 + z 6 ) +z6 T ( z ([ 5 A 6 z 6 [ + gz ) 7 ) +z7 T v4 ϕ v 6 ϕ = 6 i= ( zt i A iz i [ T [ +z7 T g 0 z4 0 g z 6 [ [ v4 J + (f v 6 J 3 + g 3 u) }} ẋ 7 where J and J are the Jacobian matrices of ϕ and ϕ respectevely, such as: [ 0 dc J (x 7 )= t ω 0 dc t ω 4 dc t ω 0 dc t ω 3 0 [ cd ω J (x 7 )= c d ω c d ω 3 c d ω 4 c t ω c t ω c t ω 3 c t ω 4 (3) Therefore, the stabilization of the whole system can be obtained by introducing a following control law: [ J u = g 3 J ( [ T [ g 0 z4 0 g z 6 [ ) } v4 + + A v 7 z 7 f 3 6 where A 7 R 4 4 is a positive definite matrix. (4) [ It should be noted that the determinant of the matrix J is 8d J c d c 3 t ω ω ω 3 ω 4. Therefore, this matrix is nonsingular when ω i > 0(i =,...,4), which is satisfied generally. While introducing the control (4) in equation () one obtains V = 7 i= zt i A i z i < 0 (5) Consequently, by using the virtual controls (5) and the real control (4), the whole system (0) is asymptotically stable. IV. SIMULATION RESULTS The proposed backstepping controller for mini-helicopter is applied here by simulation using Runge-Kutta s method with variable time step. The physical parameters for quadrotor are: m =0.50kg, d =0.4m, g =9.8m/s, I T = diag[ 3.8, 3.8, Nm.s /rad, c t = N.s /rad, c d = Nm.s /rad, K t = diag[ 3., 3., (6) N.s/m, K r = diag[ 5.6, 5.6, Nm.s/rad. I R = Nm.s /rad, a 0 = 89.63, a =6.06, a =0.0, b =80.9 The parameters (6) are obtained by using a parametric identification of a real mini-helicopter. The later will be presented in the next section. The initial conditions are: x (0) = x (0) = x 3 (0) = x 4 (0) = x 5 (0) = x 6 (0) = [0, 0 T and x 7 (0) = mg 4c t [,,, T. Initially, the helicopter is in hover flight. The reference trajectory chosen for x d (t), y d (t), z d (t) and ψ d (t) is that of the step response of the following transfer function: H(s) = (s +) 6 (7) where s is the Laplace variable. The order of the transfer function (7) was fixed at 6 in order to guarantee that the first five times derived of the desired trajectory starts from zero. Thus, this choice avoid jumps in the control signals (4). The controller parameters used in simulation are: A = A = A 3 = A 4 = A 5 = A 6 = diag[, and A 7 = diag[,,,. The Fig. shows the evolution of a quadrotor position and the selected desired trajectory for simulation. Fig. 3 show the output signals and theirs desired trajectory. It can be seen from these figures the good tracking of the desired trajectory. Moreover, we can notice an optimization of tilt angles (φ, θ) motions and a very small tracking errors (see Fig. 4). Consequently, the controller uses a minimum of energy to carry out the task. What enabled us to obtain the satisfactory control inputs u presented on the Fig. 5. The control signals are acceptable and physically realizable. 58

5 45th IEEE CDC, San Diego, USA, Dec. 3-5, 006 Fig.. Evolution of the quadrotor positions Fig. 5. Control inputs Fig. 3. Fig. 4. Position outputs Tracking errors 59 V. EXPERIMENTAL RESULTS In order to validate the proposed controller, we implemented the control law on a PC Pentium II at 00MHz, equipped with a dspace DS03 PPC real-time controller card, using Matlab and Simulink The sampling time has been fixed to t = 0.0sec, this is due to limitations imposed by the measuring device. Fig. 6 shows our experimental setup. The mechanical structure of the quadrotor is that of the four rotors mini-helicopter manufactured by Draganfly Innovations, Inc. ( The physical characteristics of this quadrotor are given by (6). The quadrotor move freely in two dimensional directions according of (ψ, z) axes. To measure the yaw angle ψ and the position z we use the CMPS03 Magnetic Compass and the SRF04 Ultra-Sonic Ranger respectively ( We had to install these sensors as far as possible from the electronic motors and their drivers because this type of sensor is very sensitive to electromagnetic noise. The acquisition of the measurement data is ensured by a PC and dspace card. In order to avoid abrupt changes in the measurement signals we have introduced numerical filters. We have used first order numerical filters. The pole of each filter was select to obtain improve the signal-to-noise ratio. The control signals are transferred to the power circuits of the four motors using the dspace card. The adjustments of the terminal motor voltages are carried out by using four IMCS 5 electronic module ( The nominal voltage of the used battery is 7volts. We performed several experiments on the real quadrotor helicopter, were the task was to control the yaw ψ angle and z position. Figures 7 and 8 shows the performance of the controller when applied to the subsystems S and S 3.Att = 0sec, all initial conditions of the quadrotor ares zeros. For this application we fixed the controller parameters at A 5 = diag[30, 30, A 6 = diag[5, 5 and A 7 = diag[,,,. The

6 45th IEEE CDC, San Diego, USA, Dec. 3-5, 006 Fig. 6. Experimental setup considered desired trajectory is of the following form: ψd (t) =sin( π 0 t) z d (t) =0.(sin( π 0 t)+) (8) The practical results of the proposed control law show the good tracking of the desired trajectories (see Fig. 7). Fig. 8 show the real control input u of the four motors. According to this figures we can note the presence of important peaks on the z position control signal. This phenomenon is the consequence of the information delivered by the ultra-sonic sensor. We specify here that the control signals are of good quality in spite of this nuisance of measurement. VI. CONCLUSION In this paper, we have presented a full state backstepping controller for a quadrotor helicopter. This process is an under-actuated system because it has six degrees of freedom while it has only four inputs (four rotors). The whole system was divided into three subsystems: an under-actuated subsystem, a fully-actuated subsystem and rotors subsystem. A backstepping control algorithm was proposed to stabilize the whole system and was able to drive a quadrotor to desired trajectory of Cartesian positions and the yaw angle. Simulation and experimentation results show the good performances of the proposed controller. Fig. 7. The yaw angle ψ and the z position: solid line and dashed line denotes respectively the real and the model response REFERENCES [ E. Altug, J. P. Ostrowski and C. J. Taylor, Quadrotor Control using Dual Cameral Visual Feedback, Proceedings of the 003 IEEE International Conference on Robotics and Automation, vol. 3, pp , 003. [ T. Madani and A. Benallegue, Backstepping Control for a Quadrotor Helicopter, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 006. [3 T. Madani and A. Benallegue, Backstepping Sliding Mode Control Applied to a Miniature Quadrotor Flying Robot, Proceedings of the 3nd Annual Conference of the IEEE Industrial Electronics Society IECON 006. [4 A. Mokhtari, A. Benallegue and A. Belaidi, Polynomial Linear Quadratic Gaussian ans Sliding Mode Observer for a Quadrotor Unmanned Aerial Vehicle, Journal of Robotics and Mechatronics, vol. 7, no. 4, pp , 005. [5 S. Bouabdallah, P. Murrieri and R. Siegwart, Design and Control of an Indoor Micro Quadrotor, Proceedings of the 004 IEEE International Conference on Robotics and Automation, 004. [6 S. Bouabdallah and R. Siegwart, Backstepping and Sliding-mode Techniques Applied to an Indoor Micro Quadrotor, Proceedings of the 005 IEEE International Conference on Robotics and Automation, pp , 005. [7 P. Castillo, A. Dzul and R. Lozano, Real-Time Stabilization and Tracking of Four-Rotor Mini Rotorcraft, IEEE Transactions on Control Systems Technology, vo.., no. 4, pp , 004. [8 T. Hamel, R. Mahony and A. Chriette, Visual servo trajectory tracking for a four rotor VTOL aerial vehicle, Proceedings of the 00 IEEE International Conference on Robotics and Automation, 00. [9 M. Vukobratovic, Applied Dynamics of Manipulation Robots: Modelling, Analysis and Examples, Berlin: Springer-Verlag, 989. [0 S. B. V. Gomes and J. J. Jr. G.Ramas, Airship dynamic modeling for autonomous operation, IEEE International Conference on Robotics and Automation, 998. [ I. Fantoni and R. Lozano, Non-linear control for under-actuated mechanical systems, Springer, 00. Fig. 8. Motor control inputs 50