Modelling and Control of the UAV Sky-Sailor

Transcription

1 Autonomous Systems Laboratory ASL Master project Modelling and Control of the AV Sky-Sailor Professor: Roland Siegwart Assistants: André Noth Samir Bouabdallah Sébastien Gros Andrea Mattio

2 INDE Introduction 5 Sky-Sailor dynamic model 7. Assumptions of the model 7. Modelling with Euler-Lagrange method 8.3 Reference frames fixation 8.4 6Do fixed mass rigid body model.5 Aerodynamic forces of an airfoil.6 Non conservative forces model 4.7 Airfoil coefficient analysis 5.7. Induced Drag 7.8 Modeling group Motor-Gearbox-Propeller 8.9 Simulink simulator.9. Notes on the simulator 3 Control model definition 3 3. Lift Drag and Moment Coefficient analysis Lift and Moment coefficient interpolation Drag coefficient interpolation 5 3. New sequence of Tait-Bryant angles Relationship between the different sequences Differential use of the ailerons 8 4 Control strategy 9 4. Choice of a method for the Low Level Control (LLC) 3 4. Choice of a method for the High Level Control (HLC) 3 5 Model linearization 3 5. Search of a steady-state point Steady-state point for a straight flight Steady-state point for a circular flight Model linearization around the steady-state point 38 6 Low level control: Linear Quadratic Regulator 4 6. Linear quadratic control 4 6. Solution of Riccati equation Linear Quadratic Regulator for Sky-Sailor Model Reduction Analysis of controllability 44

3 Andrea Mattio Master Project Summer Choice of Q and R LQR with integral action 46 7 High level control Heading generator Velocity scheduling controller for a nonholonomic mobile robot [3] rom heading to roll set point 5 8 HLC and LLC fusion: simulation 5 8. Altitude saturation and commands filter 5 8. Necessity of a HLC Sky-Sailor behavior in simulation Without wind disturbs With wind disturbs 56 9 Embedded system on Sky-Sailor DSPIC 6 9. Sensors IM (Inertial Measurement nit) Airspeed sensor Altimeter Miniature GPS 63 Control implementation on the DSPIC 64. General functional scheme 64. LLC implementation 65.3 HLC implementation 67.4 Timer organization 68 Conclusions 69. Modeling 69. Control 69 Acknowledgments 7 3 Bibliography 7 4 Annexes Least mean squares method Conventions used Relationship between different Tait-Bryant sequence: details rom angular rates to angles derivatives LLC behaviour Implementation details 8 Modelling and Control of the AV Sky-Sailor 3

4 Andrea Mattio Master Project Summer Numerical Integration with Euler method Numerical interpolation ilter implementation LLC pseudo code HLC pseudo code ourier Transformation and Power Spectral Density dspic general features Bus IC 87 Modelling and Control of the AV Sky-Sailor 4

5 Andrea Mattio Master Project Summer 6 Introduction The Autonomous System Lab of EPL is developing an ultra-lightweight solar autonomous model airplane called Sky-Sailor with embedded navigation and control system. The main goal of this project is to jointly undertake research on navigation control of the plane and also work on the design of the structure and the energy generation system. The airplane should be capable of continuous flight over days and nights which makes it suitable for a wide range of applications. In the figure is represented the general structure of Sky-Sailor. igure.: Sky-Sailor overview The main objectives of this Master Project are the following: Complete and exhaustive modelling of Sky-Sailor to obtain a good simulation model Creation of a good control model Design of a control able to stabilize the plane and to reject perturbations as wind Test and validation of the control in simulation Implementation of the control on the real system Test and validation of the control on the real system The major challenges to overtake are: Good aerodynamic knowledge of the structure Design of a control at the same robust but able to reduce consumed energy École Polytechnique édérale de Lausanne Modelling and Control of the AV Sky-Sailor 5

6 Andrea Mattio Master Project Summer 6 Design of a control robust but easy and light to implement Saving energy while implementing A great difficulty working with flying object is the danger and the risk always present during tests. Losing contact with the plane while flying at meters high is quite dangerous for the plane but not only. The reliability of the system must be as higher as possible. Modelling and Control of the AV Sky-Sailor 6

7 Andrea Mattio Master Project Summer 6 Sky-Sailor dynamic model Sky-Sailor is a model-scale solar airplane that is intended to achieve continuous flight with the only energy of the sun. It consists of a glider with a wingspan of 3. meters that is motorized by a DC motor connected to the propeller (a) through a gearbox. The control surfaces are: Two ailerons (b) on the main wing that act mainly on the roll of the airplane The V-tail (c) at the rear side composed of two control surfaces that act mainly on the pitch if they change symmetrically (elevator) and on the yaw if their deviation is not symmetrical (rudder). b c pitch y a e Y Z E roll x z yaw b igure.: Sky-Sailor. Assumptions of the model The flying systems are very difficult to model because of the quantity of dynamic and aerodynamic effects acting on them; so it s necessary to make some assumptions in order to find out a correct but at the same time simplified model of the airplane. The airplane is considered as a 6Do fixed mass rigid body on which they are acting the aerodynamic forces of lift drag and moment The centre of mass and the body fixed frame origin are assumed to coincide The structure is supposed rigid and symmetric (diagonal inertia matrix). The wind speed in the Earth frame is set to zero so that the relative wind on the body frame is only due to the airplane speed. Modelling and Control of the AV Sky-Sailor 7

8 Andrea Mattio Master Project Summer 6. Modelling with Euler-Lagrange method irst one has to model the behavior of a 6Do fixed mass rigid body moving in the space. To achieve this target it s possible to use Euler-Lagrange or Newton-Euler approach. The simpler moreover from the point of view of calculation time is Euler-Lagrange. Anyway it s useful to underline that both methods lead to the same final result. Euler-Lagrange approach uses the concepts of potential and kinetic energies in order to obtain the final equations. Γ i where (.) with d dt L L q i qi L T V is called the Lagrangian q i : generalized coordinates (.) Γ i : generalized forces dues to the non conservative forces T : total kinetic energy V : total potential energy.3 Reference frames fixation It s necessary to fix the reference frames and the conventions about angles rotations and positions in the different reference frames. Two different reference frames were used one fixed to the airplane (body reference frame) and the other one fixed to the earth (earth reference frame). rom now on we will use the following symbolic convention: e : Variable in the body reference frame E : Variable in the earth reference frame The earth-fixed reference frame is considered inertial a simplification that allows the forces due to the Earth's motion relative to a star-fixed reference system to be neglected. Modelling and Control of the AV Sky-Sailor 8

9 Andrea Mattio Master Project Summer 6 igure.: Earth-fixed and body fixed reference frames One can define the position of a point in the space with cartesian cylindrical or spherical coordinates. sually the chosen system is the cartesian one which allows to describe a rotation with Euler angles. It s useful to underline that many times Euler angles are confused with the angles of Cardano also named angles of Tait-Bryant in the aerodynamic field. Euler angles describe the following rotations: Rotation of ψ around Z : u Rotation of θ around u : Z z Rotation of φ around z : u x igure.3: Euler angles representation Tait-Bryant angles describe instead the following rotations: Rotation of φ around x : (roll angle with π < φ < π ) Rotation of θ around y : (pitch angle with π < φ < π ) Rotation of ψ around z : (yaw angle with π < ψ < π ) Modelling and Control of the AV Sky-Sailor 9

10 Andrea Mattio Master Project Summer 6 The following rotation matrixes can be written: igure.4: Tait-Bryant angles representation R cos β sin β cosγ sin γ sin γ cosγ sinα cosα sin β cos β ( x α ) cosα sinα R( y β ) R( z γ ) Multiplying the three matrixes it s possible to obtain the total rotation matrix: R ( φ θ ψ ) R( z ψ ) R( y θ ) R( x φ) R ( φ θ ψ ) cosψ sinψ sinψ cosψ cosθ sinθ sinθ cosθ cosφ sinφ sinφ cosφ R ( φ θ ψ ) cosψ cosθ sinψ cosθ sinθ cosψ sinθ sinφ sinψ cosφ cosψ sinθ cosφ + sinψ sinφ sinψ sinθ sin+ cosψ cosφ sinψ sinθ cosφ sinφ cosψ (.3) cosθ sinφ cosθ cosφ Tait-Bryant angles were chosen in the present case because they are the most used in the aerodynamic field and they reflect well the various rotations acting on the airplane as the MT IM sensor uses the same convention..4 6Do fixed mass rigid body model The first step consists of modeling the behavior of six degrees of freedom fixed mass rigid body moving in the space referred to an earth-fixed reference frame. To develop these equations one uses Euler-Lagrange formalism: Modelling and Control of the AV Sky-Sailor

11 Andrea Mattio Master Project Summer 6 with Γ i d dt L T V L L q i qi q i : generalized coordinates Γ i : generalized forces dues to the non conservative forces T : total kinetic energy V : total potential energy The kinetic energy due to the translation is immediately: E tran kin M MY + MZ + (.4) As stated it in the hypotheses one assumes that the inertia matrix is diagonal and thus that the inertia products are equal to zero. The kinetic energy due to the rotation is: Where E rot kin I xxω x + I yyω y + I zzωz (.5) ω ω ω are the rotational speeds in the body fixed reference frame that can be x y z and of the relative rates ( φ θ ψ ) useful to point out that the time variation of Tait-Bryant angles ( φ θ ψ ) expressed as a function of the roll pitch and yaw ( φ θ ψ ). It s is a discontinuous function. Thus it is different from body angular rates ( p qr ) which are physically measured with gyroscopes for instance. In the aerodynamic field usually: p ω x q ω y (.6) r ω z In general an Inertial Measurement nit (IM) is used to measure the body rotations directly calculate for the Tait-Bryant angles. ωx φ ψ sinθ ωy θ cosφ + ψ sinφcosθ (.7) ω z θ sinφ+ ψ cosφcosθ However Tait-Bryant angles representation suffer from a singularity ( θ ± π /) also known as the gimbal lock. In practice this limitation does not affect the AV in normal flight mode. Modelling and Control of the AV Sky-Sailor

12 Andrea Mattio Master Project Summer 6 It s easy to write the total kinetic energy of the system: ( M + MY + MZ + I ω + I ω + I ω ) tran rot T Ekin + Ekin xx x yy y zz z (.8) The potential energy can be expressed by: V M g Z The Lagrangian is: L T V The motion equations in the earth-reference frame are then given by: d dt d dt L L L L φ φ τ φ.5 Aerodynamic forces of an airfoil d dt d dt L L Y Y L L θ θ Y τ θ d dt d dt L L Z L L τ ψ ψ ψ (.9) (.) (.) (.) The figure below shows the section of a wing also called airfoil. The chord of the wing is the line between the leading and the trailing edge and the angle between the relative speed and this chord is the angle of attack (Aoa). As every other solid moving in a fluid at a certain speed one can represent the sum of all aerodynamic forces acting on the wing with two perpendicular forces: the lift force l and the drag force d that are respectively perpendicular and parallel to the speed vector. l Leading edge Chord length Angle of attack Relative Wind 5 % Chord d Chord Trailing edge Thickness igure.5: Aerodynamic forces on an airfoil The application point of the lift and drag forces is very close to the 5% of the chord but this can slightly change depending on the angle of attack. In order to simplify the problem the application point is considered as fixed and a moment is added to correct this assumption. Modelling and Control of the AV Sky-Sailor

13 Andrea Mattio Master Project Summer 6 ρ l Cl Sv ρ d Cd Sv (.3) (.4) ρ M Cm Sv chord (.5) with ρ : Density of fluid (air) S : Wing area v : light speed (relative to surrounding fluid) C L : Lift coefficient C D : Drag coefficient C M : Moment coefficient The lift drag and moment coefficients depend on the airfoil the angle of attack and a third value that is the Reynolds number. It is representative of the viscosity of the fluid and it can be expressed in the form: with R ρ v chord μ w n (.6) v w: relative wind speed μ : dynamic viscosity C L increases almost linearly with the angle of attack until the stall angle is reached. The wing should never work in this zone where the loosing altitude very rapidly. C L decreases importantly which makes the airplane C D has a quadratic relation to the angle of attack. Modelling and Control of the AV Sky-Sailor 3

14 Andrea Mattio Master Project Summer 6 igure.6: Relationships between C L C D and Aoa for Sky-Sailor profile.6 Non conservative forces model The non-conservative forces and moments come from the aerodynamics. On the airplane seven parts are considered as depicted on the figure below where the right and left side of each wing are divided into a portion with and without control surface. l5 M 5 d5 l4 M 7 l7 d7 l6 M 4 d4 M 6 d6 l3 M 3 d3 l M d l prop M d igure.7: Non conservative forces acting on the plane The aerodynamic forces and moments are expressed in the body fixed reference frame and they are given by: 7 tot prop + ( di + li ) (.7) i Modelling and Control of the AV Sky-Sailor 4

15 Andrea Mattio Master Project Summer 6 7 M tot ( M i + di ri + li ri ) (.8) i M i prop f ( x ) li ρ Cli Ai vi di ρ Cdi Ai vi ρ Cmi Ai chordi v i [ Cl Cd Cm ] f ( Aoa ) [ Cl Cd Cm ] f ( Aoai ) i [ Cl5 Cd5 Cm5 ] f ( Aoa5 ) [ Cl6 Cd 6 Cm6 ] f ( Aoa6 3) [ C C C ] f ( Aoa ) l7 d 7 m v i : relative wind speed in the body fixed reference frame : motor voltage : inclination angle of the ailerons and of the v-tail ( i 3 4 ) i 4 (.9) (.) The expressions of the forces and the moments have to be transferred in the earth fixed reference frame using the following relations: cosψ cosθ cosψ sinθ sinφ sinψ cosφ cosψ sinθ cosφ + sinψ sinφ E sinψ cosθ sinψ sinθ sinφ + cosψ cosφ sinψ sinθ cosφ sinφ cosψ e (.) sinθ cosθ sinψ cosθ cosφ sinθ M E cosφ cosθ sinφ M e (.) sinφ cosθ cosφ Substituting [ Y Z ] E equations of Sky-Sailor. and [ ] E.7 Airfoil coefficient analysis M φ Mθ Mψ M we finally obtain the motion The values of lift drag and moment coefficients depend on the airfoil on the attack angle and on the Reynold s number as said before. These values can be obtained with specific software (for example Javafoil []). All the considerations in this report are relative to the Walter Engel Modelling and Control of the AV Sky-Sailor 5

16 Andrea Mattio Master Project Summer 6 Airfoil that was used for the main wing. The same analysis has been made also for the NACA airfoil used for the V-tail. igure.8: Relationship between Drag Coefficient Aoa and flap angle of the aileron (obtained with Javafoil) igure.9: Relationship between Lift Coefficient Aoa and flap angle of the aileron (obtained with Javafoil) Modelling and Control of the AV Sky-Sailor 6

17 Andrea Mattio Master Project Summer 6 igure.: Relationship between Moment Coefficient Aoa and flap angle of the aileron (obtained with Javafoil) In the simulation model one directly used the values of Javaoil software. In this way we have a right value for these coefficients. C C andc obtained thanks to l d m.7. Induced Drag or the determination of the drag coefficient one has to consider also the term of the Induced Drag which is an additive term to the drag coefficient simply due to the airfoil. The induced drag can be easily written as: C A induced d r K w A r C chord π L l (.3) Aspect ratio of the airfoil (.4) As said before the drag due to the fuselage is not considered. This assumption can be done without making a big error in the model. Modelling and Control of the AV Sky-Sailor 7

18 Andrea Mattio Master Project Summer 6.8 Modeling group Motor-Gearbox-Propeller In order to build a complete and reliable model of the Sky-Sailor is necessary to model also the group Motor-Gearbox-Propeller. ω m M m ω p M p MOTOR DC GEARBO PROPELLER T x e igure.: Group Motor-Gearbox-Propeller As shown in the figure the goal is to find an analytical relationship between the thrust of the propellert the voltage applied to the motor and the relative speed in the body fixed reference frame x e. The motor chosen for the propulsion group of Sky-Sailor is a DC motor Maxon Re-5 (nr. 875) with a gearbox that has a ratio of 8.. In order to build a simplified model we have to make some hypotheses as usual: We neglect the dynamic behavior of the motor We use experimental dates to find the relationship between the thrust and the power of the propeller and the independent variables ω x The useful equations that will be used to build the model are: Ra Ra I ω m M m + MOTOR (.5) k km m p e ωm ω p r M rηm p m GEARBO (.6) Modelling and Control of the AV Sky-Sailor 8

19 Andrea Mattio Master Project Summer 6 P M p ω T f P f p ( x ω p ) ( x ω ) p PROPELLER (.7) The first step is to find an analytical expression for P f ( x ω ) p. To make this an interpolation technique is used having a set of experimental dates of the used propeller. In fact the model of the propeller is very difficult to find out and it s not completely necessary for the construction of the model. igure.: Relationship between Power of Solariane propeller Airplane Speed and Propeller Speed Interpolation can be limited to the interval: [ m ] x 4 [ m ] 6 < s < and [ rpm] < ω p < [ rpm] s To interpolate this surface a minimization with least squares technique is used and a D cubic function in the form: f ( x ω ) γ x p γ x p + γ x ω + γ x ω + γ 9 + γ x + γ p 3 4 ω 3 p + γ 5 ω p + γ 6 ω p + γ x 7 ω p (.8) Modelling and Control of the AV Sky-Sailor 9

20 Andrea Mattio Master Project Summer 6 It s possible to obtain a better result with a function of 4 th order but the difference is not big enough to justify the use of a more complex model. igure.3: Power of Solariane Propeller interpolation Once the analytical expression is obtained it s possible to develop some calculation and to find an algebraic equation between ω p (dependent variable) and (independent variable). a m R ( ) a I x ω r η ω R ω p r η + P p p (.9) k km one obtains an algebraic 3 th order symbolic equation solvable Substituting P( x ω ) f ( x ω ) thanks to MatLab solve. 3 p p p p aω + bω + cω + d (.3) p ω ω ω p p p3 g g g 3 ( x ) Re ( x ) Im ( x ) Im (.3) It s important to pay attention to the MatLab function solve because some time it gives strange results Modelling and Control of the AV Sky-Sailor

21 Andrea Mattio Master Project Summer 6 The only possible solution is g ( x ) ω because it s the only real solution. p The second step is to find an analytical expression for T f ( x ) procedure as before it s easy to obtain the following relationship: f ( x ω ) σ x p σ x p + σ x ω + σ x ω + σ Substituting this expression of g ( x ) analytical expression T f ( ) p 5 p 6. ollowing the same + σ x + σ ω + σ ω + σ ω + σ x ω p x. ω into T f ( x ω ) p p 7 p (.3) p one finally finds out an igure.4: Relationship between Thrust of Solariane Propeller Motor Voltage and Airplane Speed.9 Simulink simulator Once obtained a complete dynamic non linear model of the plane the following step is to implement it under MatLab Simulink. The simulator is built: To simulate the behavior of the non linear model achieved To validate our model (for example comparing its behavior with the one of -Plane) To design and test the controller Modelling and Control of the AV Sky-Sailor

22 Andrea Mattio Master Project Summer 6 Aerodynamic orces and Moments Calculation [ ] Y Z [ M M M ] φ θ ψ 6Dof fixed mass rigid body motion equations igure.5: General Simulink model The 6Dof fixed mass rigid body motion equations block implements the dynamic equations obtained with Euler-Lagrange formalism and the various coordinate changes due to the relative rotations between the earth fixed reference frame and the body fixed reference frame. The Aerodynamic orces and Moments Calculation block implements the calculus of the orces acting on the seven parts of the wings referred to the reference frame fixed to the plane..9. Notes on the simulator To build a simulator in MatLab Simulink is quite easy. There are only some aspects to keep in mind to obtain a proper simulator. avoid algebraic loops that don t allow proper simulation use a correct and uniform simulation time pay attention to the signs Modelling and Control of the AV Sky-Sailor

23 Andrea Mattio Master Project Summer 6 3 Control model definition p to now a model useful for the simulation has been built. The next step is to build a model suitable for the control. It s so necessary to make some simplification and some change in order to create a model that can be easily used to design a good controller for our system. Three main changes have to be made. Interpolation of the Lift Drag and Moment Coefficient se of another Tait-Bryant angles sequence more suitable to the path tracking chosen Differential use of the ailerons 3. Lift Drag and Moment Coefficient analysis As said before the values for the airfoil coefficients are obtained thanks to specific software (Javafoil). To build a control model it s necessary to find out an analytical relationship between the values of the aerodynamic coefficients and the independent variables (attack angle Reynolds number and aileron angle). The relationships are quite difficult to express analytically. The solution chosen is the interpolation. As usual some hypotheses have to be made: Reynolds number is considered constant and equal to A range of aileron angle between - and is considered A range of attack angle between -5 and 5 is considered 3.. Lift and Moment coefficient interpolation The process of interpolation for the lift and moment coefficients is very similar. In fact it s easy to see in the following figures that in the considered range both the lift and the moment coefficients can be expressed as a linear D function of the attack angle and the flap angle. Modelling and Control of the AV Sky-Sailor 3

24 Andrea Mattio Master Project Summer 6 igure 3.: Lift and Moment coefficients in the reduced range The easiest way to find this relation is to interpolate this surface with a plane of equation: f ( Δ α a) Δ α + Δ a + 3 (3.) α : angle of attack a : aileron flap angle To find the values of Δ i : constant values Δ i there are different possibilities. The first and the simplest one is to find a plane passing for three points of the surface. The second one is to interpolate the surface with a plane exploiting the least squares method. In this case it has been chosen to use the first method that being more empirical give better results. igure 3.: Lift coefficient interpolation Modelling and Control of the AV Sky-Sailor 4

25 Andrea Mattio Master Project Summer 6 igure 3.3: Moment coefficient interpolation 3.. Drag coefficient interpolation or the Drag coefficient the situation is different. In fact the surface can t be interpolate with a linear D function. igure 3.4: Drag coefficient in the reduced range Modelling and Control of the AV Sky-Sailor 5

26 Andrea Mattio Master Project Summer 6 The idea is to use a quadratic D function to interpolate this surface. A general quadratic D function can be expressed as: f ( + f λ ) λ' Q λ + Γ λ (3.) q q Q q q γ Γ γ α λ a f const In order to find a function of this kind that fits well our surface a minimization technique as the least squares or an empirical way can be used. The disadvantage linked to the minimization with the least squares is that being a process purely mathematical it s possible to obtain values of the Drag coefficient that are inferior to zero that is physically impossible. So a way to find a good solution is to use a mix of the two techniques: use of the least squares minimization and correction of the wrong values thanks to an empirical and visual analysis. igure 3.5: Drag coefficient interpolation Modelling and Control of the AV Sky-Sailor 6

27 Andrea Mattio Master Project Summer 6 3. New sequence of Tait-Bryant angles The goal is to keep the plane flying consuming the minimum energy quantity. The strategy chosen is to make it following a circle. So the plane it s always in a turning configuration. If the Tait-Bryant sequence seen in the previous chapter is used at each instant a variation of the rotation matrix ( φ θ ψ ) R occurs. A possible solution is to use the following sequence: R cosθ sinθ cosψ sinψ sinψ cosψ sinφ cosφ sinθ cosθ ( x φ) cosφ sinφ R( y θ ) R( z ψ ) The new sequence used is the following: R ( φ θ ψ ) R( z ψ ) R( x φ) R( y θ ) cosθcosψ - sinθsinφsinψ - cosφsinψ sinθcosψ + cosθsinφsinψ R ( φ θ ψ ) cosθsinψ + sinθsinφcosψ cosφcosψ sinθsinψ - cosθsinφcosψ (3.3) - sinθcosφ sinφ cosθcosφ At the same time another relationship between the angular rates ω ω ω ) and the velocities ( φ θ ψ ) has to be found. ( x y z ω θ θ φ x cos - sin cos φ ω φ y sin θ (3.4) ω sinθ cosθ cosφ z ψ 3.. Relationship between the different sequences It s naturally useful to pass easily from one to another sequence. So it s necessary to find a transformation between the two sequences. The following notations will be used: ( θ ψ ) R st st st st φ for the first sequence used to build the simulation model ( θ ψ ) R nd nd nd nd φ for the second sequence used to build the control model There is a set of ( θ ψ ) angles ( φ θ ψ ) st st st φ that describes the same orientation as the set of nd nd nd. The angles of course are different but the orientations are the same. This fact gives a way to convert angles in one scheme to or from angles in another. All nine of the matrix elements are the same because the orientations are the same. sing the following equality: Modelling and Control of the AV Sky-Sailor 7

28 Andrea Mattio Master Project Summer 6 ( φ θ ψ ) R ( φ θ ψ ) R nd nd nd nd st st st st one can find a way to pass from one sequence to the other. The used relationships are the following. All the calculations are reported in Appendix. φ θ ψ nd nd nd ( cosθ sinφ ) arcsin st st sinθ arctan st cosθst cosφst sinψ st cosφst cosψ arctan cosψ st cosφst + sinψ st st sinθ sinθ st st sinφst sinφ To implement this transformation on the DSP that has a limited capacity calculus it s possible to linearize it considering as correct the approximation of little angles for θst and φ st. Making this approximation: φ θ ψ nd nd nd φ θ st st sinψ arctan cosψ st st cosψ + sinψ st st 3.3 Differential use of the ailerons θ θ st st φst φ st Another useful simplification is to suppose a differential use of the ailerons. This it made to simplify the control action on the roll and to avoid strange correction of the controller on the pitch or on the yaw using aileron flap. So on 3 and the system will be considered as a system with 4 and not with 5 inputs. new motor aileron 3 v tail 4 v tail left right st (3.5) (3.6) (3.7) Modelling and Control of the AV Sky-Sailor 8

29 Andrea Mattio Master Project Summer 6 4 Control strategy The control is organized in two different levels with different functions and different targets. Low level control(llc): High level control(hlc): Stability of the system LQR(Linear Quadratic Regulator) Path planning and tracking Modern approach HLC LLC SKY-SAILOR Inner Loop Outer Loop igure 4.: Control strategy The target of the inner loop is to keep the stability of the system while the one of the outer loop is to plan a path (in our case a circle) and to track it. The Low Level Control has to satisfy the following main requirements: Keeping the global stability of the system Keeping a constant air speed Keeping a constant height ollowing the roll set point given by the HLC Keeping the stability also in presence of a wind thought as a disturb Reducing as far as possible the energy consumption Minimizing the solicitations to the servo mechanism The High Level Control instead: Path planning Path tracking Modelling and Control of the AV Sky-Sailor 9

30 Andrea Mattio Master Project Summer 6 4. Choice of a method for the Low Level Control (LLC) The most used control method in the avionic field is the classical PID. The need in this case is to have a control with the following requirements: Robust and able to reject perturbations as wind or thermals Possible to implement on a DSP without a great calculation power The final choice is to use an optimal linear state feedback control method based on the built dynamic and aerodynamic model and in particular a LQR (Linear Quadratic Regulator). 4. Choice of a method for the High Level Control (HLC) The target to achieve is to keep the plane flying following a specified path with low energy consumption. The choice is to follow a circular path with a large diameter in order to avoid shocks on the commands (ailerons v-tails and motor) and to reduce wind influence. In literature there are a lot of papers proposing method to achieve this goal. As usual the problem is to use a control method requiring a little calculation power and easy to implement. The choice is to adapt an algorithm [3] proposed and tested for the path tracking of a nonholonomous robot. Modelling and Control of the AV Sky-Sailor 3

31 Andrea Mattio Master Project Summer 6 Modelling and Control of the AV Sky-Sailor 3 5 Model linearization To design the control a linear model of the system is needed. This linear model will be naturally correct only around the so-called steady-state point which has to be found. The steps to achieve are: inding the steady-state point Linearize the model around this point 5. Search of a steady-state point 5.. Steady-state point for a straight flight The final non-linear model obtained thanks to modeling process is in the form: ) ( f (5.) 4 3 z y x z y x E E E E E E ψ θ φ ψ θ φ The goal is to find a particular configuration of and for which a steady-state solution of the equation (5.) is obtained that means: ) ( f (5.) irst some hypotheses can be made to simplify the problem. All these hypotheses are made on the base of physical considerations about airplane behavior during his flight.

32 Andrea Mattio Master Project Summer 6 x y z E E E??? (5.3) They are insignificant for the search of the steady-state point because they don t influence the dynamic behavior of the airplane. The part of the model describing the evolution of the position of the airplane in the Earth fixed reference frame is totally decoupled in respect of the part of the model describing the evolution of the other states. φ θ? ψ (5.4) The airplane in its steady-state configuration should have a roll and a yaw angle equal to zero. Anything is known about the pitch angle. The pitch angle influences the angle of attack of the airplane so it s a free parameter. pitch y roll x z yaw igure 5.: Plane angles configuration x y z E E E? The speed E x is approximately.[ m s] (5.5) 8 from considerations coming from the reality. Anyway this is surely a free parameter in the search of the steady-state point. Modelling and Control of the AV Sky-Sailor 3

33 Andrea Mattio Master Project Summer 6 φ θ ψ (5.6) It s quite evident that the angular rates should be equal to zero. or the inputs the reasonable hypotheses that at the steady-state point the aileron are closed can be made. The motor voltage instead can be considered as a free parameter. Anyway also in this case some information about the possible range of values for the parameter is available. The plane has a glide slope in a stable situation of approximately 5. With a simple calculation: glide _ slope 5 T N M g Looking at figure (.) one can see that this corresponds to a motor voltage of approximately9 V. This calculation is just an indication and it s not a reference to judge the results. 3 4? (5.7) 5... Steady-state point empirical search One important characteristic of Sky-Sailor is that is auto-stable. So a first essay to find an indication about the steady-state point is to simulate with the MatLab Simulink simulator the free behavior of the airplane. Simulating the free behavior of the airplane with the initial conditions found before it s possible to see that after an initial transitory the Sky-Sailor reaches a stable situation. irst the motor voltage is set equal to zero that it means that the airplane is left in a situation of free fall and the following results are obtained: Modelling and Control of the AV Sky-Sailor 33

34 Andrea Mattio Master Project Summer 6 igure 5.: θ evolution in free fall condition After some beginning oscillations the airplane stabilizes itself for an angle θ igure 5.3: z E evolution in free fall condition Modelling and Control of the AV Sky-Sailor 34

35 Andrea Mattio Master Project Summer 6 or the position z E in 5sec the plane falls of approximately5 m. One can make an estimation of the fall speed that is 5 m z.3 m. This result is indicative but it s quite 5sec sec similar to the one obtained during the tests with the real plane. or the speed a value of x e 8.4 m is found that it s quite consistent with the values obtained sec in the reality. Then a motor voltage is put in order to observe the behavior of the airplane. A motor voltage of9 V is set because it s approximately the voltage necessary to allow the airplane to fly in a steady-state condition. igure 5.4: θ evolution with motor voltage 9V In this situation an angle θ. 5 is obtained. The optimal situation should be an angle of - because of the particular configuration and disposition of the v-tail. So one can conclude that thanks to the built model a static behavior similar to the real one is found. A speed x e 8.5 m that is quite consistent with real experiments is found. sec Modelling and Control of the AV Sky-Sailor 35

36 Andrea Mattio Master Project Summer Steady-state point mathematical search A second possible approach to find the steady-state point is a mathematical approach thanks to an optimization procedure. The starting point is the non-linear model f ( ). In this model the following values are imposed: φ ψ y E z E 3 φ 4 θ ψ The free parameters are: x E? θ?? (5.8) (5.9) The goal is to find the values of these free parameters that minimize the cost function: J x φ E + ye + ze + + θ + ψ (5.) This procedure is realized thanks to the MatLab function fmincon. The result obtained is: x m E sec θ V These results are quite consistent with those obtained with the empirical method so one can conclude that the steady-state point found is reasonable and it reflects quite well the behavior of the real system. 5.. Steady-state point for a circular flight As said before the goal is to keep the plane flying following a circular path. In these flight conditions is better to fin a steady-state point appropriate. The procedure is similar to the one used in the chapter 5... The parameters imposed are the following: Modelling and Control of the AV Sky-Sailor 36

37 Andrea Mattio Master Project Summer 6 x E ψ 8.5m s y E z E φ θ The free parameters are: φ? θ? ψ??? 3?? 4 In this case the cost function J is more complicated than (5.). J (5.) (5.) x E + ye act + ze + φ + θ + + W W roll ail (5.3) ψ The acceleration along y E has to be modified because of the fact that the plane is turning so it s influenced by a centripetal acceleration. To estimate in an easy way this term one can have a look at the following figure: x E R c y x igure 5.5: Evaluation centripetal acceleration It s easy to see that the acceleration along can be estimated as: ye will be equal to the centripetal acceleration which Modelling and Control of the AV Sky-Sailor 37

38 Andrea Mattio Master Project Summer 6 a ct x R E c (5.4) The goal is also to minimize the energy dissipated so at the steady-state point the wings should be almost closed. To achieve this a steady-state point for which a minimum action on the aileron and on the v-tail is required. So it s necessary to add the condition: W + + (5.5) 3 4 At the same time the plane shouldn t work in roll so it s necessary to minimize the value of the roll forces acting on the ailerons. Another is introduced: W roll ail sing the optimization function of MatLab fmincon the following results are obtained: φ -.68 θ 4. ψ V.4.5 (5.6) 5. Model linearization around the steady-state point Once found the steady-state point the following step consists of linearizing the model around this point. There are different methods to linearize a model and the so-called tangent linearization has been chosen. The goal in fact is to obtain the matrixes A and B to design later the controller with LQR technique. The goal is to write the linear model in the form: A + B A general tangent linearization is in the form: ( ) f A B f ( ) (5.7) (5.8) This simple direct method is not applicable in the case of the Sky-Sailor model because of the dimensions of the equations describing the model. In fact MatLab is unable to solve the direct problem. So we have to implement a different method. Modelling and Control of the AV Sky-Sailor 38

39 Andrea Mattio Master Project Summer 6 Modelling and Control of the AV Sky-Sailor 39 A possible solution is to split the calculation in different little calculations. M M M M M M M M M f M f M f f A ) ( ) ( ) + + (5.9) 7 7 ) ( i i i i M M (5.) or the linearity of the sum and the derivation one can write: 7 7 i i i i i i M M (5.) ) ( ) ( ) i i M Mi i i i i i i i i M Mi i i i i i i M M M M M f M f M f f A (5.) nfortunately this simplification is not sufficient. It s necessary to go deeper in this procedure. C Mi C Mi C Di C Di C Li C Li M i M i i i Mi Di Li Mi Mi i CMi CMi CDi CDi CLi CLi Mi Mi i i Mi Di Li Di Di i C Mi C Mi C Di C Di C Li C Li M i M i i i Mi Di Li Li Li i C Mi C Mi C Di C Di C Li C Li M i M i i i Mi Di Li i i i i C C C C C C C C C C C C C C C C C C M (5.3)

40 Andrea Mattio Master Project Summer 6 Modelling and Control of the AV Sky-Sailor 4 Substituting 4.7 in 4.6 the final relation that leads to the matrix A of the linear model can be found. The same procedure is used also to build the matrix B. But for the matrix B is sufficient to stop this procedure at a higher level. M M M M M M M M M f M f M f f B ) ( ) ( ) + + (5.4) Implementing this algorithm in MatLab it s possible to arrive at the solution of the problem finding the matrix A and B of the linear model around the steady-state point found before.

41 Andrea Mattio Master Project Summer 6 6 Low level control: Linear Quadratic Regulator 6. Linear quadratic control Linear quadratic control is a technique of optimal state feedback control. Each linear or linearized system can be written in the form: x ( t) Ax( t) + Bu( t) y( t) Cx( t) + Du( t) The Linear Quadratic Control consists of finding a static matrix K in order to find the input to the system: u( t) K x( t) (6.) that has to stabilize the system and to minimize the cost function: J T T ( x Qx + u Ru) dt where Q and R are two weight matrixes which obey to the following constraints: Q Q T R R (6.4) We can find the solution: K R B T P T where P is solution of the Riccati equation: PA A T P + PBR B T P + Q P It s possible to consider a steady-state solution of this equation as usually made in literature. So the equation to solve is: (6.) (6.3) (6.5) (6.6) PA + A T P PBR B T P + Q (6.7) 6. Solution of Riccati equation Now the problem is to solve the Riccati equation 5.7. A general Riccati equation can be written as: where: B A D C (6.8) r + r r r Modelling and Control of the AV Sky-Sailor 4

42 Andrea Mattio Master Project Summer 6 A B C r r r [ nxn] [ nxm] [ mxn] D r The Hamiltonian matrix can be built: Ar H Cr Br D r that in this case is equivalent to: [ mxm] [ mxn] (6.9) A H Q BR B T A T (6.) It s possible to demonstrate that the n eigenvalues of H are the n eigenvalues of the closed loop matrix λ will be too. A B K and their opposites. inally if λ is an eigenvalue of H it s sure that So it exists n and only n eigenvalues whose real part is negative. If Λ diag( λ... λ n ) is the matrix of the eigenvalues one can build the matrix [ nxn] made of eigenvectors and one obtains the following important relationship: H Λ (6.) inally it s possible to divide in two matrixes and and to demonstrate that P desired solution of the Riccati equation is in the form: P (6.) 6.3 Linear Quadratic Regulator for Sky-Sailor 6.3. Model Reduction The final linearized model of the system is in the form: A + B Y C + D where: [ xe ye ze x E y E z E φ θ ψ φ θ ψ ] [ ] 3 4 (6.3) (6.4) Modelling and Control of the AV Sky-Sailor 4

43 Andrea Mattio Master Project Summer 6 Modelling and Control of the AV Sky-Sailor 43 In reality it s not necessary to control all the states of this model but at the same time it s necessary to control some state which is not in this model. To make it possible a spacetransformation can be made in order to project the system in the desired space of controllability. So the transformation: Γ ˆ (6.5) is made with: [ ] θ φ θ φ E e e E z y x z ˆ (6.6) The states that must be controlled are: φ Roll θ Pitch E z Altitude e x Air speed e y Lateral slip The other three states are considered in this model reduced to maintain the mathematical controllability of the system. The transformation matrix Γ should be well-conditioned and non-singular ( det Γ ). It s possible to demonstrate that the following matrix is well-conditioned and non singular. + Γ sin cos cos sin cos - cos sin - cos sin sin sin cos sin sin sin - cos cos φ ψ φ ψ φ φ θ ψ φ θ ψ θ ψ φ θ ψ θ Inversing (6.5) one obtains: ˆ Γ (6.7)

44 Andrea Mattio Master Project Summer 6 Considering the matrix Γ evaluated in the steady-state point one obtains a numerical matrix and it s possible to conclude that: Γ ˆ (6.8) Substituting in (6.3): Γ ˆ AΓ ˆ + B ΓAΓ ˆ ˆ + ΓB The target is finally achieved because a reduced model is obtained in the form: ˆ A ˆ ˆ + B ˆ ˆ (6.9) (6.) (6.) Â (6.) ˆB (6.3) 6.3. Analysis of controllability The first thing to make is to check if the reduced model is controllable or not. A system is controllable if: rank( Q) n System dimension where : Modelling and Control of the AV Sky-Sailor 44

45 Andrea Mattio Master Project Summer 6 n B Q [ B AB A B... A ] Controllability matrix To check the controllability is sufficient to use the MatLab instruction ctrb 3. With this analysis it s possible to find out that the system is controllable Choice of Q and R As one can read in literature it doesn t exist a methodical approach for the choice of the weight matrixes Q and R. This is the weak point of this control method and it s the point that adds a difficulty to a method apparently so easy to use. irst it s good thing to choice to use only diagonal matrixes in order to simplify the control design. Q diag q ) with i 8 ( i R diag r ) with j 4 ( j The choice has been made observing the behavior of the control in simulation. The goal is to design a control with the following characteristics: Air speed always constant with a few variations allowed Robust to wind disturbs Action on the ailerons as slighter as possible Motor use reasonable inally a good choice for the matrixes is the following: Q (6.4) 3 One can read in MatLab help that: Estimating the rank of the controllability matrix is ill-conditioned; that is it is very sensitive to roundoff errors and errors in the data. So it s preferable to use ctrbf for problems of big dimensions Modelling and Control of the AV Sky-Sailor 45

46 Andrea Mattio Master Project Summer 6 R The control obtained behaves quite well as far as concerns the stability 4. (6.5) LQR with integral action With the LQR control a static error on some states appears. In particular a static error on the airspeed x E on the roll φ is present. This error can be quite negative for the control behavior so it s necessary to correct it. The possible solution is to add an integral term. One way of forcing integral action is to put a set of integrators at the output of the plant. In other words adding an integral term in a state-feedback control method means augmenting the system. This can be described in a state-space form as: x y z () t Ax() t + Bu() t () t Cx() t () t y() t r(t) The composite system becomes: where: () t A x () t + B u () t (6.6) x (6.7) () t x () () A B x t A z t C B (6.8) Then the goal is to find the gain K that stabilizes this augmented system. In the system an integral action is put on: φ Roll x e Air speed So the augmented system (6.7) is built the controllability is checked and the good matrixes Q and R are found. This technique has the advantage of being robust to variations in system matrix (ABCD) that are used to compute the steady-state point. 4 All the figures concerning intermediate results are reported in the Annexes Modelling and Control of the AV Sky-Sailor 46

47 Andrea Mattio Master Project Summer 6 Modelling and Control of the AV Sky-Sailor 47 A A (6.9) B B (6.3) It s easy to find out that the augmented system is controllable and after some trial good matrixes Q and R are found for the new system. The aspects to consider in designing this controller are the same made in the previous chapter Q (6.3) R (6.3)

48 Andrea Mattio Master Project Summer 6 The final control that will be tested in simulation and implemented on the real system is the following: K Modelling and Control of the AV Sky-Sailor 48

49 Andrea Mattio Master Project Summer 6 7 High level control The target of the outer loop is to give an appropriate set point to the inner loop in order to be able to follow the desired path. The idea is to follow a simple path without consuming a lot of energy and avoiding the problems relied to the wind. The path chosen is a circle; in particular a point expressed in GPS coordinates which is the circle centre and a radius are assigned to the HLC which has to track the desired circle. The HLC should be able to track the path also in presence of wind. y RADIS CENTER x igure 7.: Path planning for the HLC Considering the position of the centre and the radius of the circle it creates the reference path. Comparing the real position and the real heading with the desired position it generates the value of heading necessary to achieve the desired position. The heading value is taken as input of a PI controller in order to generate the roll set point which is communicated to the LLC. ( x c y c ) radius Reference Path generation ( x GPS y GPS ) Heading GPS Heading generator Heading SP PI Roll SP igure 7.: HLC strategy Modelling and Control of the AV Sky-Sailor 49

50 Andrea Mattio Master Project Summer 6 7. Heading generator 7.. Velocity scheduling controller for a nonholonomic mobile robot [3] The heading generator constitutes a revision an adaptation to the plane of the Velocity Scheduling Controller for a nonholonomic mobile robot realized at the Automatic Laboratory of the EPL (École Polytechnique édérale de Lausanne). The concept is to compare the behavior of a nonholonomic mobile robot with the behavior of our plane Nonholonomic mobile robot vs Sky-Sailor Consider a mobile robot moving on a planar surface where x is the horizontal coordinate and x the vertical one. The angle that the robot makes with the horizontal axes is x 3. The kinematic equations of motions are given by: x x x 3 v v v cos sin ( x3 ) ( x ) 3 where v and v are the inputs; v denotes the velocity in the direction defined by the heading angle and v the angular velocity. The idea is that a plane moving at a given height with a given and fixed angle of attack can be described by the same kinematic equations of a nonholonomic robot (7.). In fact we can write: x y E E x e x e cos sin ( ϕ) ( ϕ) where ϕ 5 is the input; x e denotes the air speed of the plane xe and (7.) (7.) ye the positions in the GPS reference frame. Exploiting this equality the same algorithm implemented for the nonholonomic mobile robot can be used also for Sky-Sailor. The only difference is that while for the robot v can be used as an input and it s possible to act on it to achieve the target for the plane this is not possible because 5 ϕ is the heading and it s defined as the direction of the velocity vector so it can be mathematically derived as ϕ arctan y E xe Modelling and Control of the AV Sky-Sailor 5

51 Andrea Mattio Master Project Summer 6 the air speed x e is a parameter to control carefully. So the same algorithm can be used but it s mandatory to block the action on the velocity. The proposed controller is the following: Circle trajectory generation freq set_pt_wind_speed_f / (*PI*radius); sin_traj sin(*freq*pi*time); cos_traj cos(*freq*pi*time); x radius*cos_traj; xd -*freq*pi*radius*sin_traj; xdd -4*freq*freq*PI*PI*radius*cos_traj; x radius*sin_traj; xd *freq*pi*radius*cos_traj; xdd -4*freq*freq*PI*PI*radius*sin_traj; Heading generator chi_kplus (-k*k*k*int - 3*k*k*(latitude_rel_meter -x) - 3*k*(chi-xd) + xdd) * + chi; chi_kplus (-k*k*k*int - 3*k*k*(longitude_rel_meter-x) - 3*k*(chi-xd) + xdd) * + chi; int_kplus (latitude- x) * + int; int_kplus (longitude-x) * + int; x3_hat atan(chichi); last_errx3 errx3; errx3 atan(sin(x3_hat-heading_gps)cos(x3_hat-heading_gps)); 7.. rom heading to roll set point Once the heading value necessary to track properly the trajectory is generated it s necessary to convert it into a value suitable for the Low Level Control. The easiest thing to do is to convert it into a roll reference. In fact to track a heading position the easiest thing is to rotate along the longitudinal axes of the plane that in this case is represented by the x axes. But a rotation around x axes corresponds to a modification of the roll. So it s easy to transform the heading reference into a roll reference simply with a proportional gain. In reality to avoid static errors it s chosen to design a simple PI controller in order to pass from heading to roll set point. Heading SP PI Roll SP igure 7.3: rom heading to roll set point Modelling and Control of the AV Sky-Sailor 5

52 Andrea Mattio Master Project Summer 6 8 HLC and LLC fusion: simulation The last target to achieve consists of fusing HLC and LLC together in order to have a total autonomous control of the plane. The integration of two controls doesn t consist only of a simple superposition of the two levels but also in a careful analysis of the relationships between the two controllers and of the effects of one on the other. In this case the integration has lead to make some variations to the LLC in particular to vary some weight in order to achieve a better behavior of the system. Design of a control which doesn t solicit to much the servo-mechanism. This design is made thanks to the ourier analysis of the input signals of the system. 6 Saturation on ( z ) z < to lighten the control and to avoid abrupt changes on the z max physical inputs of the system. iltering on the input values given to the system to avoid high frequencies solicitations of the servo-mechanism. 8. Altitude saturation and commands filter Altitude saturation is necessary to lighten control action and aggressiveness. Being an optimal but always a proportional controller LQR reacts in a very aggressive manner in front of important errors on the states. This is really dangerous in the case of the altitude for the global behavior of the system; in fact the risk of going in instability it s great. or that reason it s useful to saturate the value of the altitude. igure 8.: Altitude control with and without saturation 6 The results of this analysis are reported in the Annexes Modelling and Control of the AV Sky-Sailor 5

53 Andrea Mattio Master Project Summer 6 igure 8.: Total path without and with saturation A filter on the commands is useful in order to reduce the solicitations on the servos. But at the same it s a very delicate point to touch. In fact design a wrong filter on the commands means to lead the system to instability. The chosen one is a Butterworth first order filter with a cut frequency of 3Hz. This frequency has been chosen because it s more or less the maximum excited frequency for the servos. 8. Necessity of a HLC The first simple idea to follow a circular path is to give a constant roll to the plane in order to make it turning. This technique being simple and easy to implement is not adapt to Sky-Sailor. In fact in presence of a little constant wind (m/sec in direction) the system behavior is really different to the hoped one. igure 8.3: Plane behavior in presence of a constant wind with only LLC control Modelling and Control of the AV Sky-Sailor 53

54 Andrea Mattio Master Project Summer 6 The plane begins to fly following a spiral; this is why a HLC control is necessary. In fact with the insertion of the HLC control in the same wind situation the plane behavior is really better. igure 8.4: Plane behavior in presence of a constant wind with HLC and LLCcontrol HLC control takes one or two rounds to fit well the circle but then also in presence of a wind it s able to keep the desired circular path. 8.3 Sky-Sailor behavior in simulation Thanks to Simulink simulator it s possible to analyze the behavior of the plane in absence and in presence of the wind. At the same it s also possible to simulate the noise present on the sensors measures and so to understand quite well the possible real behavior of Sky-Sailor. It s important to underline that the wind is modeled in a quite simple way: wind influence is simulated as an increment or a reduction on the plane speed in the earth reference frame. igure 8.5: wind simulated Modelling and Control of the AV Sky-Sailor 54

55 Andrea Mattio Master Project Summer Without wind disturbs Simulations are first made without considering wind disturbs. In this first simulation the plane has the initial conditions: x m e 8. ψ 7 z 3m x m sec igure 8.6: HLC and LLC control without wind disturbs While LLC keeps the stability of the plane and controls the altitude HLC works to achieve the circle. This is evident looking at the following two figures representing the plane x-z so LLC action and the plane x-y so HLC action. igure 8.7: x-y and x-z planes Modelling and Control of the AV Sky-Sailor 55

56 Andrea Mattio Master Project Summer 6 It s possible to conclude that HLC-LLC fusion control works quite well in absence of wind. It s interesting to analyze also the influence as far as concerns solicitations to the servos. igure 8.8: Servos solicitations 8.3. With wind disturbs Second simulation has been made considering wind effect and in particular wind has been simulated as in figure 8.3. The initial conditions are the same that for the previous simulation. igure 8.9: HLC and LLC control with wind disturbs Modelling and Control of the AV Sky-Sailor 56

57 Andrea Mattio Master Project Summer 6 System behavior is quite good considering flight difficult conditions. This simulation shows a good robustness of the total control. igure 8.: x-y and x-z planes igure 8.: Controlled altitude Once more it s possible to say that total control has a correct behavior also in presence of a quite strong wind. In fact if one considers that the plane flies with a constant speed of 8. m/sec the wind is equal to: m wind _ strength% sec 4.4% 8. m sec Modelling and Control of the AV Sky-Sailor 57

58 Andrea Mattio Master Project Summer 6 or servos solicitations it s possible to see that wind disturbs leads to an augmentation of them. igure 8.: Solicitations in wind presence Anyway it s right to conclude that these solicitations are sustainable for the plane also because the goal is to fly in good weather situations and not in presence of repeated wind flows. Modelling and Control of the AV Sky-Sailor 58

59 Andrea Mattio Master Project Summer 6 9 Embedded system on Sky-Sailor The main components of the embedded system on Sky-Sailor are: DSPIC: it constitutes the heart of the embedded system where all the information is collected and where the control is implemented. This element has a calculation power not so elevated and this constitutes one of the main challenges in the control design. ONBOARD SENSORS: they constitute the receptors of the system and they allowed the knowledge of all the useful information for the control BS IC: it allows communication between some sensor and DSPIC RADIO MODEM: it s used for the communication between the ground and the plane igure 9.: Embedded system on Sky-Sailor Modelling and Control of the AV Sky-Sailor 59

60 Andrea Mattio Master Project Summer 6 9. DSPIC The dspic Digital Signal Controller (DSC) from Microchip is a powerful 6-bit (data) modified Harvard RISC machine that combines advantages of a high-performance 6-bit microcontroller with the high computation speed of a fully implemented digital signal processor (DSP) to produce a tightly coupled single-chip single-instruction stream solution for embedded systems design. The dspic used has the following characteristics: SRAM EEPROM Timer ADC Device Pins Bytes Instructions bytes bytes 6-bit -bit dspic364a 8 44K 48K ch The term expressing the speed of the DSPIC is the MIPS (Million of Instructions Par Second). This term is really important and it s expressed the number of instruction that the DSP engine (heart of the structure) is capable to make. Naturally this term is tied up to the frequency of the processor hence to the frequency of the PLL chosen. In fact for the dspic chosen it s possible to choose the frequency of the PLL (x4 x8 x6). It s evident that lower frequency means lower power consumption. To program the dspic code C has been used and then transformed to Assembler thanks to MPLAB IDE. 9. Sensors 9.. IM (Inertial Measurement nit) The IM used is the MTx produced by -Sens. This is a complete miniature inertial measurement unit with integrated 3D magnetometers (3D compass) with an embedded processor capable of calculating roll pitch and yaw in real time as well as outputting calibrated 3D linear acceleration rate of turn (gyro) and (earth) magnetic field data. Modelling and Control of the AV Sky-Sailor 6

61 Andrea Mattio Master Project Summer 6 igure 9. : Inertial Measurement nit All calibrated sensor readings (accelerations rate of turn earth magnetic field) are in the right handed Cartesian coordinate system as defined in igure (?). This coordinate system is bodyfixed to the device and it defines the sensor coordinate system (S). igure 9. : IM coordinate system The MTx calculates the orientation between the sensor-fixed coordinate system S and a earthfixed reference coordinate system G. By default the local earth-fixed reference coordinate system used is defined as a right handed Cartesian coordinate system with: positive when pointing to the local magnetic North Y according to right handed coordinates (West) Z positive when pointing up The 3D orientation output is defined as the orientation between the body-fixed coordinate system S and the earth-fixed coordinate system G using the earth-fixed coordinate system G as the reference coordinate system. Modelling and Control of the AV Sky-Sailor 6

62 Andrea Mattio Master Project Summer 6 igure 9. : Example of IM coordinate system It s important to underline the measures obtained thanks to the use of the IM and what s their role for the control implementation. Roll ( φ ) Pitch ( θ ) Yaw ( ψ ) Gyroscope ( ω ω ω ): they are used to obtain the values of ( φ θ ψ ) the control x y z useful for Magnetometer: information coming from it it s not used for the control implementation Accelerometer ( a a a ) x y slip. This term is simple obtained making an integral z : acceleration on y it s used to derive the value of the lateral 9.. Airspeed sensor The airspeed sensor is simply a pressure transducers with temperature compensation which is able to give an accurate measure of the pressure. The pressure value is then converted to an airspeed value thanks to the formula: p ρ v The sensor chosen has pressure range of -5 mbar with a sensitivity of 64 counts/mbar. This gives a way to find the sensibility for the airspeed. In particular it s possible to find out that for values of airspeed between 8 and 9 m/s the sensibility is of. m/s. This value of sensibility is quite good for control implementation. (9.) Modelling and Control of the AV Sky-Sailor 6

63 Andrea Mattio Master Project Summer 6 igure 9. : Airspeed sensor The values coming from the airspeed sensor are sent to the DSPIC thanks to the bus IC Altimeter To obtain the value of altitude the sensor used is a barometer; so once more the pressure value is transformed into an altitude value thanks to the simple formula: p ρ g h (9.) igure 9. : Barometer Also in this case it s possible to obtain an indication of the sensibility of the sensor. Making some easy calculation it s possible to find out that the sensibility is of more or less m Miniature GPS The absolute position is given by an ultra-low power GPS sensor with patch antenna from Nemerix. This sensor consumes only 6mW for a weight of.36 g. In term of position accuracy 95% / 99.7% of the time the error is of more or less m. The data are sent to on a serial port at a fixed rate of Hz to a microcontroller that decodes the NMEA protocol stores the value internally and send them on demand to the main processor via IC. 7 Some information about IC is available in the annexes Modelling and Control of the AV Sky-Sailor 63

64 Andrea Mattio Master Project Summer 6 Control implementation on the DSPIC In the following chapter only functional schemes of control implementation are reported. or a more complete understanding of the real implementation (for example: integration method interpolation method ) details are reported in the Annexes.. General functional scheme High Level Control (HLC) Latitude-Longitude and Heading measures received from GPS Measures adaptation to the reference frame used for the control Execution of the HLC algorithm and definition of the roll-setpoint Execution frequency: Hz Execution time: 3ms Low Level Control (LLC) Roll-Pitch received from IM Angular rates received from IM (Gyroscope) Acceleration received from IM Wind Speed received from pressure sensor Altitude received from pressure sensor Measures analysis (saturation and rescue function) Measures adaptation to the reference frame used for the control Execution of the LLC algorithm definition of commands values Commands filtering Commands saturation Commands sending to the servo Execution frequency: 5Hz Execution time:.5ms igure.: Control functional scheme In the figure the general functional scheme of the control implementation is reported. It s possible to calculate the percentage of time spent evaluating control algorithm each second as: Modelling and Control of the AV Sky-Sailor 64

65 Andrea Mattio Master Project Summer 6 (.5ms 5 + 3ms) % control 6.55% (PLL set to x4) ms One can conclude that the time used to evaluate the algorithm control is quite little and this is really good for the general behavior of the DSPIC.. LLC implementation Measures received from sensors: Roll Pitch Angular rates Acceleration Wind Speed Altitude abs(roll) < 3 & abs(pitch) < 3 & Wind Speed < 5m/s & Wind Speed > 7 m/s NO Set commands to neutral position: u_ail u_vtail_l u_vtail_r u motor YES Roll-Pitch transformation to control reference frame Transformation from angular rates to φ and θ Integration of the acceleration to obtain lateral slip Roll Set Point interpolation Set point subtraction: fixed for all the states except the roll (roll set point comes from HLC) Modelling and Control of the AV Sky-Sailor 65

66 Andrea Mattio Master Project Summer 6 Saturation to z at. m LQR Linear Quadratic Regulator Commands Digital iltering: Exponential Low Pass Commands transformation to servomechanism commands Commands < Maximum Rates NO Commands saturation YES Sending to the servo STOP igure.: LLC functional scheme Modelling and Control of the AV Sky-Sailor 66

67 Andrea Mattio Master Project Summer 6 The algorithm of LLC is executed each.4 sec (at a frequency of 5 Hz). The measures coming from sensors are available each. sec (at a frequency of Hz). The solution adapted is to use for the control the average of the four values available between one execution of the control loop and the next one..3 HLC implementation HLC algorithm is executes each second so at a frequency of Hz because it needs information coming from GPS which are available only each second. Measures received from GPS: Latitude Longitude Heading GPS ok? NO ixed roll SetPoint equal to 4 YES Latitude and longitude transformation into relative meters coordinates Heading generator Circle trajectory generation PI control Roll eventual saturation STOP igure.3: HLC functional scheme Modelling and Control of the AV Sky-Sailor 67

68 Andrea Mattio Master Project Summer 6.4 Timer organization Timer organization is really important for a correct implementation of the control system. In fact the measures coming from the IM are available at a frequency of Hz those coming from the GPS and from the altimeter at Hz; LLC is executed at 5Hz and HLC at Hz. All the timers are synchronized with the signal coming from the GPS at Hz which constitutes the reference time. When this signal arrives all the timers are set to zero. At a frequency of Hz the measures from IM arrive; at each measure arrival a timer5hz is augmented till it reaches the value of 4. At this point the averaged measures are used to execute the LLC and the timer5hz is reset to zero. This procedure is made with interruption and not with polling. Each Hz instead the HLC is executed hence when the signal coming from GPS is available. Timer organization has the following simple structure: Signal Hz Timer5Hz Timer5Hz ++ Timer5Hz 4 Execute LLC igure.4: Timer organization Modelling and Control of the AV Sky-Sailor 68