Dynamic Feedback Control for a Quadrotor Unmanned Aerial Vehicle N. K. M Sirdi, Abdellah Mokhtari LSIS Laboratoire de Sciences de l Information et des Systèmes, CNRS UMR 6168. Dom. Univ. St- Jérôme, Av. Escadrille Normandie-Niemen 13397. Marseille Cedex 20. France University of Science and Technology Oran Algérie. email: msirdink@free.fr Résumé A nonlinear kino- dynamic model for a quadrotor unmanned aerial vehicle is presented with a comparison of state parametrizations based on Euler angles and positions states. The first method, called partial state representation, emphasizes the control of roll, pitch and yaw angles rather than the second one is dedicated to drive translational motions of the UAV. The system is presented as two cascaded parts, the first one for the rotational motion and the last for translations. A dynamic feedback control is applied to linearize the closed loop first part leading a controllable and decoupled subsystem. Performance and robustness of the proposed controller are analysed in simulation. Mots-clés Dynamic Feedback Control, Partial State Feedback, Full State Feedback, Unmanned Aerial Vehicle, nonlinear estimation I. INTRODUCTION The characteristics of the UAV system components are determined by some key operational requirements like endurance, radius of action, altitude and take on/off landing. Their successful application depends on their level of controllability and flying qualities [1]. Fully autonomous operations and faster dynamics lead to difficulties in control design to obtain a satisfactory level of performance. A systematic study of the UAV dynamics representation seems to be necessary. Small scale helicopters with fully autonomous operation depends essentially on controllability and flying qualities. The difficulty of the control design is due in general to complexity of dynamic representation of the quadrotor, and how exactly it reflects the real system. Hence this dynamic must take into account a set of important parameters affecting the behavior of the system, in order to understand the performance requirements of controlling such class of unmanned aerial vehicles. The quadrotor helicopter dynamic unstabillity comes from parameters uncertaities and wind disturbances. Therefore suitable control methods are needed to make them stable, robust and performant. Erdinç Altug et. al [2] present two methods of control of the quadrotor, the first one using linearizing feedback control, and the other using back-stepping-like control law. Both approaches have been used to control translational motion (x, y, z) and yaw angle (ψ). They need high order lie derivatives (for the first method) and huge calculation (for second method). Vincent Mistler et. al [3][4] developped the dynamic model in non linear state space representation, and used an exact linearization and non-interacting control for the global system to evaluate translational motion and yaw angle outputs. We have shown that the input-output decoupling problem is not solvable for this model by means of a static state feedback control law. Then, a dynamic feedback controller was developed to render the closed-loop system linear, controllable and noninteractive after a change of coordinates in the state-space. Finally, the stability and the robustness of the proposed control law in the presence of wind, turbulences and parametric uncertainties was analyzed. A delay of control inputs here seems to be necessary to avoid singularity and make the problem solvable. In a previous paper we have presented a kino dynamical model of a four rotors helicopter [5]. For such model, we have shown that it is possible to apply a non linear controller with observers based on the Backstepping approach. This has shown a good robustness and permit to reduce the number of the required sensors that have to be used, by observing the absolute velocity of the helicopter which is difficult to measure. Performance and characteristics of a Luenberger observer, combined with a classical polynomial controller (based on an accurate model of the plant), have been analyzed in [6]. The observer is efficient when dealing with bounded uncertainties, disturbances and noise. The analysis is based on the observer and tracking errors during transients and at steady state, and on the performance and robustness with respect to plant uncertainties. Estimation of wind parameters have been added to reinforce the robustness. In the present work two ways Fig. 1. The quadrotor Aerial Vehicle (UAV) model. of representation both of them using feedback linearization has been used and analyzed to control a full and partial dynamic system. However there are some fundamental technical problems which can be identified: understanding the aerodynamics of vertical flight and knowing The theoretical power required to produce a fixed amount of lift.
Keeping structural weight and engine weight down such that the machine could lift a payload. Conquering the problem of vibrations. This was a source of many mechanical failures of rotors and airframe. The complexity of control design for underactuated systems is that they are not fully feedback linearizable. Moreover, many recent and traditional methods of nonlinear control design including backstepping [7] [8], forwarding [9] [10] [11] [12], high-gain/low-gain designs [13] [10], and sliding mode control [14] are not directly applicable to underactuated systems with the exception of a few special cases. II. DYNAMIC REPRESENTATION The equations describing the attitude and position of an UAV (represented in figure 1) are basically those of a rotating rigid body with six degrees of freedom [15] [16]. They may be separated into kinematic equations and dynamic equations [17]. The kinematic equations may be represented as follows. The absolute position of the UAV is described by the three coordinates (x 0, y 0, z 0 ) of its center of mass with respect to an earth fixed inertial reference frame and its attitude by the three Euler s angles (ψ,θ,φ). These three angles are respectively called yaw angle ( π ψ < π), pitch angle ( π 2 < θ < π 2 ) and roll angle ( π 2 < φ < π 2 ). The derivatives with respect to time of the angles (ψ,θ,φ) can be expressed in the form col( ψ, θ, φ) = N(ψ, θ, φ)ω (1) in which ω = col(p, q, r) is the angular velocity expressed with respect to a body reference frame and N(ψ, θ, φ) is the 3x3 rotation matrix given by N(ψ, θ, φ) = 0 sin φ sec θ cos φ sec θ 0 cos φ sin φ 1 sin φ tan θ cos φ tan θ The dynamic equations are now expressed. Using the Newton s laws about the center of mass, one obtains the dynamic equations for the miniature four rotors helicopter 1 m V 0 = F ext (2) J ω = ω Jω + T ext m is the mass, J is the inertia matrix given by J = I x 0 0 0 I y 0 0 0 I z and F ext, T ext represent the vector of external forces and external torques respectively. They contain 1 denotes the usual vector product and Cφ = cos(φ); Cψ = cos(ψ); Sθ = sin(θ) the helicopter s weight, the aerodynamic forces vector, the thrust and the torque developed by the four rotors. Some calculations yield the following form for theses two vectors Fext = A x (CφCψSθ + SφSψ)u 1 A y (CφSθSψ CψSφ)u 1 A z + mg (CθCφ)u 1 (3) Text = in which A p + u 2 d A q + u 3 d A r + u 4 col(a x, A y, A z ) and col(a p, A q, A r ) are the resulting aerodynamic forces and moments acting on the UAV and are computed from the aerodynamic coefficients C i as A i = 1 2 ρ airc i W 2 (ρ air is the air density, W is the velocity of the UAV with respect to the air) [17]. g is the gravity constant (g = 9.81ms 2 ); d is the distance from the center of mass to the rotors; u 1 is the resulting thrust of the four rotors; u 2 is the difference of thrust power between the left rotor and the right rotor (y direction); u 3 is the difference of thrust power between the front rotor and the back rotor (x direction); u 4 is the difference of torque between the two clockwise turning rotors and the two counter-clockwise turning rotors. Each rotor undergoes thrust and torque and of course leaves a wake behind as it moves. Assuming that the electric motors are velocity controlled, then (u 1, u 2, u 3, u 4 ) may be considered directly as control inputs. The real control signals (u 1, u 2, u 3, u 4 ) have been replaced by (ū 1, ū 2, ū 3, ū 4 ) to avoid singularity in lie transformation matrices when using exact linearization [10]. In that case u 1 has been delayed by double integrator. The Input-Output linearization uses full state feedback to globally linearize the nonlinear dynamics of selected controlled outputs (for the newsystem; see figure 2). Each of the output channels is differentiated a sufficient number of times until a control input component appears in the resulting equation. The others control signals are keept unchanged [3][4][5]. u 1 = ζ u 2 = ū 2 ζ = ξ u 3 = ū 3 ξ = ū 1 u 4 = ū 4 Using the Lie derivative, feedback linearization will transform the nonlinear system into a linear and noninteracting system which is known as Brunovsky form: d 4 x 0 dt 4 = v 1 d 4 y 0 dt 4 = v 2 (4) d 4 z 0 dt 4 = v 3 d 2 ψ dt 2 = v 4
v 1, v 2, v 3, v 4, represent the new input control signals. Obviously the state vector need the primary state ( x 0, y 0, z 0, ψ) and their successive derivatives to be compared with the desired state trajectories when developing the control law. On adopting a classical polynomial based control law, the closed loop system will be represented by the following equation: v 1 = x (4) d λ 3 (x (3) 0 x (3) d ) λ 2(ẍ 0 ẍ d ) λ 1 (ẋ 0 ẋ d ) λ 0 (x 0 x d ) v 2 = y (4) d λ 3 (y (3) 0 y (3) d ) λ 2(ÿ 0 ÿ d ) λ 1 (ẏ 0 ẏ d ) λ 0 (y 0 y d ) (5) v 3 = x (4) d λ 3 (z (3) 0 z (3) d ) λ 2( z 0 z d ) λ 1 (ż 0 ż d ) λ 0 (z 0 z d ) v 4 = ψ d λ 5 ( ψ ψ d ) λ 4 (ψ ψ d ) The choice of λ i is decisive in defining the dynamic of the system since the poles placement is based on. The closed loop with complete state feedback can be represented as in Figure 2. Fig. 2. Full state closed loop system K 1 = I x cos(θ) θ ψ + cos(θ) 2 sin(φ) ψ 2 I z cos(φ) cos(θ)sin(φ) 2 ψi z θ + cos(φ) 2 θiz cos(θ) ψ cos(φ) θ 2 I z sin(φ) cos(θ) 2 cos(φ) ψ 2 I y sin(φ) cos(θ)cos(φ) 2 ψi y θ + sin(φ) 2 θiy cos(θ) ψ + sin(φ) θ 2 I y cos(φ) K 2 = I y ψsin(θ)sin(φ) θ + I y ψcos(θ)cos(φ) φ I y sin(φ) φ θ cos(θ)cos(φ) ψ 2 I x sin(θ) + cos(θ)cos(φ) ψi x φ + sin(φ) θix sin(θ) ψ sin(φ) θi x φ + sin(θ) ψ 2 I z cos(θ)cos(φ) sin(θ) ψi z sin(φ) θ φi z cos(θ)cos(φ) ψ + φi z sin(φ) θ K 3 = I z ψsin(θ)cos(φ) θ I z ψcos(θ)sin(φ) φ I z cos(φ) φ θ sin(θ) ψ 2 I y cos(θ)sin(φ) sin(θ) ψi y cos(φ) θ + φi y cos(θ)sin(φ) ψ + φi y cos(φ) θ + cos(θ)sin(φ) ψ 2 I x sin(θ) cos(θ)sin(φ) ψi x φ + cos(φ) θix sin(θ) ψ cos(φ) θi x φ To avoid complications on control aim and to not fall in an underactuated system, it is necessary to set the number of input equal to the number of output channels. Since the input signals are u 1, u 2, u 3, u 4 the output signals to be controlled are chosen as : III. PARTIAL STATE REPRESENTATION This representation is based on rotational motion (tilt angles) rather than translational motion. This technique has been adopted for different reasons: 1. Tilt angles, velocities and accelerations can be well measured by inertial sensors accurately. 2. The dynamic equation which relates yaw, pitch, roll angles to external torques represent a full rank system so singularity is avoided directly and there is no need to delay control inputs by double integration. 3. An observer can be used to estimate positions (x, y) through the controlled tilt angles output and thrust power input. 4. The altitude z 0 is also controlled since it is is directly affected by thrust power and roll-pitch angles, and its construction through an observer is not possible. With the state vector x 1 = col(ψ, θ, φ) ; and the state vector x 2 = col(x 0, y 0, z 0 ) we may write the dynamic equation of the system as: M (x 1 ) ẍ 1 + K(x 1, ẋ 1 ) = T ext (6) M (x 1 ) = K(x 1, ẋ 1 ) = [K 1 ; K 2 ; K 3 ] T mẍ 2 = F ext (7) I xsin(θ) 0 I x I y cos(θ)sin(φ) I y cos(φ) 0 I z cos(θ)cos(φ) I z sin(φ) 0 y c = col(x 0, y 0, z 0, ψ) To encounter the translational motion measure which is rather difficult and instrumentation in this case (GPS, camera,...) are not accurate enough, we have chosen to use the following measured signals for control: y m = col(ψ, θ, φ, z 0 ) So the system to be controlled can be written: [ m ] [ ] [ m CθCφ 0 z0 + CθCφ g ] = 0 M(x 1 ) ẍ 1 K(x 1, ẋ 1 ) (8) The feedback linearization control is applied to the equation (8) with inputs u 1, u 2, u 3, u 4 and outputs ψ, θ, φ, z 0. If one consider no disturbance on the system (A p = A q = A q = A z = 0), the control law will then become u 1 u 2 u 3 u 4 = D 1 D = m C(θ)C(φ) (v 1 g) M(x 1 ) v 2 v 3 + K d 0 0 0 d 0 0 0 1 v 4 A z CθCφ +u 1 A p +u 2 d A q +u 3 d A r +u 4
M(x 1 ) is a non singular matrix. Hence the system is transformed from nonlinear system into a linear and controllable one, so the closed loop system is reduced to four double integrators: z 0 = v 1 ψ = v 2 θ = v 3 φ = v 4 Let the tracking error signals be as follows: e z = z 0d z, ė z = ż 0d ż e ψ = ψ d ψ, ė ψ = ψ d ψ e θ = θ d θ, ė θ = θ d θ e φ = φ d φ, ė φ = φ d φ condition problem when solving equations of position (x 0, y 0, z 0 ). So the desired trajectory is filtered with a third order filter defined by the transfer function H f (p) to make it smooth and have zero initial conditions before exciting system. The chosen reference trajectories are given by : x 0d = 1 2 cos( t 2 ); y 0d = 1 2 sin( t 2 ) z 0d = 1 t 10 ; ψ d = π 3 0.125 H f (p) = p 3 + 1.5p 2 + 0.7p + 0.125 the constant parameters of the quadrotor are: From equation of forces and taking into account only the measured signals, the relations between desired positions and desired angles (the desired roll angle φ d (ẍ 0d, ÿ 0d, ψ d, u 1 ) and the desired pitch angle θ d (ẍ 0d, ÿ 0d, ψ d, u 1 )) are derived from: [ ] [ ] ẍ0d (cos φd cos ψ m = d sin θ d + sin φ d sin ψ d ) u 1 ÿ 0d (cos φ d sin θ d sin ψ d cos ψ d sin φ d ) u 1 The reference signals are given by: Fig. 4. Angle trajectories for ψ, θ, φ and tracking errors Fig. 3. Partial state feedback controlled system v 1 = z 0d + λ v ė z + λ p e z v 2 = ψ d + λ v ė ψ + λ p e ψ v 3 = θ d + λ v ė θ + λ p e θ v 4 = φ d + λ v ė φ + λ p e φ The differential equation for the tracking error is then given as follows: ë z + λ v ė z + λ p e z = 0 ë ψ + λ v ė ψ + λ p e ψ = 0 ë θ + λ v ė θ + λ p e θ = 0 ë φ + λ v ė φ + λ p e φ = 0 if λ v and λ p are chosen to assign a specific set of eigenvalues such that the system will converge exponentially. The closed loop system with partial state feedback can be represented as in Figure 3. IV. SIMULATION RESULTS For observability requirements the reference trajectory proposed here is chosen in a manner to avoid initial Fig. 5. Position trajectories for x, y, z and their tracking error m = 2Kg, I x = I y = I z = 1.2416N/rad/s 2, d = 0.1m, g = 9.81m/s 2 The control parameters are such as λ 0 to λ 3 and λ 4, λ 5 are the coefficients of the polynomials (p + 5) 4 and (p + 5) 2 respectively and: λ v = 16; λ p = 64
Fig. 6. Control input signals u 1, u 2, u 3, u 4 Fig. 8. Partial state feedback controlled system A. Flight without perturbation Results without perturbation are shown in Fig-4 to Fig-6 for full state feedback and in Fig-7 and Fig-8 for partial state feedback. Simulation results for desired and observed positions for both methods are presented: Fig. 9. Tracking errors for x, y, z Fig. 7. Partial state feedback controlled system B. Flight with parametric uncertainties For parameters uncertainties of -20% on values of m, Ix, Iy, Iz and d, are considered. Robustness of the quadrotor control is shown below in Fig 9,Fig-10): C. Compensation of wind disturbances: Parameters Ap, Aq, and Ar have been introduced in control law to analyze the behavior of the system when taking into account perturbation in the controller. For Ap = 0.02, Aq = 0.03, Ar = 0.04 the following results are obtained (Fig-12 to Fig14) : D. Discussion on simulation results It is seen from reference trajectories tracking without perturbation (Fig-3) that feedback linearization control on Euler angles is acceptable since the tracking of angles and positions was perfect (Fig5,6). This may be confirmed by the angle tracking errors (Fig-7) and the position tracking error (Fig-8) which vanished on time. The control signal figure (Fig 8) shows a smooth curve, this is due to the choice of optimal control gain λ v and λ pḣowever the position trajectory (x, y) presents a bounded tracking error (Fig 4). The tracking error in the full state feedback is more fast vanishing (Fig6), but control signals are more oscillatory An uncertainties on inertia coefficients and mass of the quadrotor of 20% were taken to analyze the robustness of the control law. Note that all this parameters affect the angle dynamic equations which are presented in closed loop form. These variations were successfully compensated by controller. from Fig-10 representing the angle tracking error and Fig-11 representing the position tracking error it is seen that the actual output ψ, θ, φ, z converge perfectly to the desired output, however the actual output x, y converge with a small error less than 1%. A study of the system with the presence of wind perturbation was presented. An estimation of the wind parameters through Lyapunov function and adaptation gain was elaborated. The tracking errors for Euler angles and positions were presented in Fig-12 and Fig-13. The convergence were confirmed for angles whereas the position errors reveal a small oscillation of 1%. The estimated wind parameters for a nominal values of A p = 0.02; A q = 0.03 and A r = 0.04; are shown in Fig-11.
Fig. 10. Angle tracking errors for 20% uncertainties on m, I x, I y, I z Fig. 12. Angle tracking errors with with disturbance for Ap = 0.02; Aq = 0.03; Ar = 0.04 Fig. 11. Position tracking errors for 20% uncertainties on m, I x, I y, I z V. CONCLUSION A partial state feedback control has been applied and compared with full dynamic state feedback and has been shown the same performance and stability when using observation of outputs (x 0, y 0 ), without need of sensors to measure them. However the system behavior toward aerodynamic forces disturbance (A x, A y ) and moment disturbance(a p, A q, A r, A z ) seems to be quite different. In the first one disturbances the system is more sensitive to variation than the other, since the variation on (A p, A q, A r ) can be easily compensated by control law extension, however (A x, A y ) will affect the forces dynamic model of the displacement (x 0, y 0 ) which are represented in an open loop form and are not easy to compensate directly by the same controller as used here. It was clearly shown that feedback linearization based control will not be robust enough toward wind disturbances. This will be true either when taking a partial state feedback control for (x 0, y 0, z 0, ψ) or for (ψ, θ, φ, z 0 ) outputs. This is because of partial feedback linearization method does not really reflect the nonlinear complete system dynamics since it transforms the partial system into a set of cascaded integrators (Brunovski s form). However the full state feedback system control or partial state control system with observers, will give satisfactory desired trajectory tracking without external disturbances. Our further investigation will be based on robust full state feedback implementable non linear control to obtain a good robustness versus load hanges and wind perturbations. Note that linear control like H, GH, µ synthesis can also be implemented but seem robust approaches like sliding modes seem to be more interesting. RÉFÉRENCES [1] B. Mettler, T. Kanade. Attitude control optimization for. a smallscale unmanned helicopter. AIAA Guidance,Navigation and Control Conference, Denver, Colorado, August 2000. [2] E. Altug, J. P. Ostrowski, R. Mahony. Control of a Quadrotor Helicopter Using Visual Feedback. International Conference on Robotics and Automation 2000. [3] V. Mistler, A. Benallegue, N. K. M Sirdi, Exact Linearization and Noninteracting Control of a 4 Rotors Helicopter via Dynamic Feedback, IEEE 10th IEEE International Workshop on Robot-Human Interactive Communication (September 18-21,2001 Bordeaux and Paris). Proceedings ROMAN Sept. 2001 Bordeaux-Paris [4] V.Mislter, A. Benallegue and N. K.M Sirdi : Linéarisation exacte et découplage entrées-sorties, Comparaison entre l hélicoptère standard & l hélicoptère 4 rotors. CIFA 2002. [5] L. Mederreg, F. Diaz and N. K. M sirdi. Non linear backstepping control with observer design for 4 rotors helicopter. AVCS 2004 Genova November 2004. [6] A Mokhtari, N K. M Sirdi, K Meghriche, A. Belaidi. Feedback Linearization and Linear Observer for a Quadrotor Unmanned Aerial Vehicle. Internal report. to be published. [7] A. Isidori. Nonlinear Control Systems. Springer, 1995. [8] M. Krstic, I. Kanellakopoulos, and P. Kokotovic. Nonlinear and Adaptive Control Design. John Wiley & Sons, 1995. [9] F. Mazenc and L. Praly. Adding integrations, saturated controls, and stabilization for feedforward Systems. IEEE Trans. on Automatic Control, Vol 40, 1996. [10] R. Sepulchre, M. Jankovic, and P. Kokotovic. Constructive Nonlinear Control. Springer-Verlag, 1997. [11] A. R. Teel, A nonlinear small gain theorem for the analysis of control systems with saturation. IEEE Trans. on Automatic Control, Vol.40, pp. 1256-1270, 1996. [12] A. R. Teel. Using Saturation to stabilize a class of singleinput partially linear composite systems. IFAC Nonlinear Control System Design Symposium, NOLCOS 92, June 1992. [13] R. Sepulchre. Slow peaking and low-gain designs for global stabilization of nonlinear systems. 36th IEEE Conference on Decision and Control, pp. 3491-3496, 1997. [14] H. K. Khalil. Nonlinear Systems. Prentice-Hall, Inc., 1996. [15] M. Vukobratovic, Applied Dynamics of Manipulation Robots: Modelling, Analysis, and Examples. Berlin: Springer-Verlag, 1989. [16] S. B. V. Gomes, J. J. Jr. G.Ramas. Airship dynamic modelling for autonomous operation. IEEE International Conference on Robotics and Automation, 1998. [17] H. Nijmeijer, A.J. Vander Shaft. Nonlinera Dynamic Control System, Springer Verlag 1990. [18] L. M. Milne-Thomson, Theoretical Aerodynamics, Dover, 1958 4th edition. [19] R. Olfati-Saber, Nonlinear Control of Underactuated Mechanical Systems with Application to Robotics and Aerospace Vehicles, PHD thesis in Electrical Engineering and Computer Science, Massachusetts Institute Of Technology, Feb 2001. [20] C.A Desoer, M.Vidyasagar, Feedback System: Input-Output Properties. Academic Press, New york, 1975 [21] O. Shakernia, Y. Ma, T. J. Koo, J. Hespanha, and S. S. Sastry, Vision Guided Landing of an Unmanned Air Vehicle. Proc. IEEE Conf. Decision and Control, Phoenix, AZ, December 1999. [22] J.Gordon Leishman,Principles of helicopter aerodynamics, university of maryland,university press,2000 [23] A. Gessow, G. Myers, Aerodynamics of the helicopter, Frederick Ungar Publishing Co, New York, thirdedition, 1967. [24] J.C. Avila-Vilchis, B. Brogliato, A. Dzul, R. Lozano, Nonlinear modelling and control of helicopters, Automatica, 2003. [25] P Castillo, A. Dzul, R. Lozano, Real Time stabilization and tracking of a four rotor mini-rotorcraft. [26] O. Shakernia, Y. Ma, T. J. Koo, and S. S. Sastry. Landing an Unmanned Air Vehicle: Vision-based Motion Estimation and Nonlinear Control, Asian J. Control, Vol. 1, No. 3, 1999 [27] I. D. Landau and R. Horowitz. Applications of the Passive Systems Approach to the Stability Analysis of Adaptive Controllers for Robot Manipulators. Int. J. of Adaptive Control and Signal Processing, V3, 1989.