Computergestuurde Regeltechniek exercise session Case study : Quadcopter

Transcription

1 Coputergestuurde Regeltechniek exercise session Case study : Quadcopter Oscar Mauricio Agudelo, Bart De Moor (auricio.agudelo@esat.kuleuven.be, bart.deoor@esat.kuleuven.be) February 5, 016 Proble description The ai of this exercise session is to control the position and attitude (aircraft orientation relative to the vehicle s center of gravity) of a quadcopter by anipulating the angular velocities of its rotors. You will have to design an LQG control syste that drives the quadcopter to several checkpoints as fast as possible, within a roo that is 6 eters wide, 6 eters long, and 3 eters high. A checkpoint is declared reached when the quadcopter enters a sphere around it. The radius of this sphere (0.08 ) is the axiu aditted error. In addition, the control syste should be robust enough to guarantee that the quadcopter can reach the checkpoints in tie when carrying a sall payload. 1 Quadcopter dynaics Aquadcopterisanaerialvehiclethatisabletohover. Ithasfour identicalrotorsarranged at the corners of a square body, and its propellers or blades have a fixed angle of attack. Figure 1 shows the diagra of the quadcopter that you will have to control. Notice that the rotors are paired, and each pair rotates in a different direction. Motors 1 and 3 rotate clockwise when looked fro above, whereas otors and 4 have a counter-clockwise rotation. When all the otors rotate at the sae angular velocity, the torques τ 1, τ, τ 3 and τ 4 (these are the counter torques applied to the aircraft as a consequence of the otors rotation) will cancel each other out and the quadcopter will not spin about its z b -axis ( ψ = 0). The quadcopter will hover when the angular velocities are such that the total thrust (f 1 +f +f 3 +f 4 ) generated by the rotors is equal to the force of gravity. In order describe the oveent of the quadrotor and its attitude, two fraes of reference are used, naely, the inertial frae and the body frae (see Figure 1). The inertial frae is defined by the ground, with gravity pointing in the negative z direction. The body frae is defined by the orientation of the quadcopter, with the rotor axes pointing in the positive z b direction and the ars pointing in the x b and y b directions. The attitude of the quadcopter is deterined by three angles, naely, roll-φ, pitch-θ and yaw-ψ. The way of changing these angles by playing with the angular velocities of 1

2 z Motor f z b y f 3 Motor 3 τ τ 3 f 1 y b q x b f f 4 y x Motor 1 τ 1 τ 4 Motor 4 Inertial Frae Body Frae Figure 1: Quadcopter configuration. The roll, pitch and yaw angles are denoted by φ, θ and ψ respectively. Motors 1 and 3 rotate clockwise and otors and 4 rotate counter clockwise as indicated by the arrows with black dotted lines. f 1, f, f 3 and f 4 are the thrusts generated by the rotors about their centers of rotation. τ 1, τ, τ 3 and τ 4 are the torques applied to the aircraft (counter torques) as a consequence of the spinning of the rotors. the rotors is illustrated in Figure. Changes in the roll and pitch angles are accopanied by translational otion. It should be clear that a quadcopter is an underactuated vehicle, since it only has 4 actuators (rotors) for controlling 6 degrees of freedo (three translational, x, y, z and three rotational, φ, θ and ψ). Let s denote the angular velocities of the otors by ω 1, ω, ω 3 and ω 4, and the angular velocity to which the quadcopter hovers by ω hover. Figure shows soe quadcopter otion scenarios which are explained in the following lines: The rolling otion corresponds to a rotation of the quadcopter about the x b -axis. It is obtained when ω = ω 4 = ω hover and ω 1 and ω 3 are changed. For a positive rolling, we have to set ω 1 > ω hover and ω 3 < ω hover. A negative rolling action is produced when we set ω 1 < ω hover and ω 3 > ω hover. The pitch otion corresponds to a rotation of the quadcopter about the y b -axis. It is obtained when ω 1 = ω 3 = ω hover and ω and ω 4 are changed. For a positive pitch, we have to fix ω > ω hover and ω 4 < ω hover. A negative pitch action is generated when we fix ω < ω hover and ω 4 > ω hover. The yaw otion corresponds to a rotation of the quadcopter about the z b -axis. It is produced by the difference in the torque developed by each pair of rotors. Since the rotors are paired, two create a clockwise torque and two an anticlockwise one; by varying the angular speed of one pair over the other, the net torque applied to

3 Hover Positive Roll Positive Pitch Positive Yaw Takeoff Negative Roll Negative Pitch Negative Yaw Angular velocity for hovering ( ω hover ) Angular velocity saller than ω hover Angular velocity larger than ω hover Figure : Illustration of the quadcopter otion obtained by varying the angular velocities of its rotors, M, and M 4. the aircraft changes which results in the yaw otion. For a positive yaw, we have to set (ω 1 = ω 3 ) > ω hover and (ω = ω 4 ) < ω hover. A negative yaw action is achieved when we fix (ω 1 = ω 3 ) < ω hover and (ω = ω 4 ) > ω hover The vertical takeoff and landing otions (change in the z-axis) are obtained by equally augenting or diinishing the angular speed of all otors with respect to ω hover. Quadcopter odel The translational otion of the quadcopter in the inertial frae is described by the following set of equations: ẍ 0 ÿ = z 0 g +RT b +F D (1) where x, y and z are the coordinates of the position of the quadcopter in the inertial frae, is the ass of the syste, g is the acceleration due to gravity, F D is the drag force due to air friction, T b R 3 is the thrust vector in the body frae, and R R 3 3 3

4 is the rotation atrix that relates the body frae with the inertial frae and is defined as follows: cosψcosθ cosψsinθsinφ cosφsinψ sinψsinφ+cosψcosφsinθ R = cosθsinψ cosψcosφ+sinψsinθsinφ cosφsinψsinθ cosψsinφ. sinθ cosθsinφ cosθcosφ The drag F D due to air friction is odelled as a force proportional to the linear velocity in each direction, ẋ F D = k d ẏ. ż Here, k d is the air friction coefficient. The thrust f i generated by the ith rotor is given by the following expression, f i = kω i, for i = 1,...,4, where k is the propeller/rotor lift coefficient and ω i is the angular velocity of the ith otor. The total thrust T b generated by the 4 rotors (in the body frae) is given by 4 0 T b = f i = k 0. 4 i=1 i=1 ω i In this study it is assued that the dynaics of the otors is uch faster than the one of the quadcopter, and therefore is not taken into account. Since the angular velocity of each rotor is typically proportional to the applied voltage, we have that ω i = c v i, for i = 1,...,4, where c is a constant and v is the voltage applied to the rotor. While it is convenient to have the linear equations of otion in the inertial frae, the rotational equations of otion are useful to us in the body frae, so that we can express rotations about the center of the quadcopter instead of about the inertial center. To this end we can use the Euler s equations for rigid body dynaics, which are defined as follows: I ω +ω (Iω) = τ () where denotes cross product, I R 3 3 is the inertia atrix, ω = [ω x ω y ω z ] T is the angular velocity vector, and τ = [τ φ τ θ τ ψ ] T is the vector of external torques. We can odel the quadcopter as two thin unifor rods crossed at the origin with a point ass (otor) at each end. This results in a diagonal inertia atrix of the following for: I = I xx I yy I zz where I xx, I yy and I zz are the oents of inertia of the quadcopter about the x b, y b and z b axes respectively. After coputing the cross product, equation () reduces to: I xx 0 0 ω x τ φ (I yy I zz )ω y ω z 0 I yy 0 ω y = τ θ (I zz I xx )ω x ω z. (3) 0 0 I zz ω z τ ψ (I xx I yy )ω x ω y 4,

5 The roll τ φ and pitch τ θ torques are derived fro standard echanics as follows: τ φ = L(f 1 f 3 ) = Lk ( ( ) ω1 ω3) = Lkc v 1 v3 τ θ = L(f f 4 ) = Lk ( ( ω ω4) = Lkc v v4), where L is the distance between the rotor and the quadcopter center (radius). As it was discussed earlier, all the rotors apply torques to the aircraft about its z b -axis while they rotate. In order to have an angular acceleration about the z b -axis, the total torque generated by the rotors has to overcoe the drag forces. The total torque about the z b -axis, that is the yaw τ ψ torque is given by the following equation: τ ψ = b ( ( ) ω1 ω +ω3 ω4) = bc v 1 v +v3 v4 where b is the propellers drag coefficient. The roll, pitch and yaw rates are related to the coponents of the angular velocity vector by eans of the following expression: φ θ ψ = Qω = 1 sinφtanθ cosφtanθ 0 cosφ sinφ 0 sinφ/cosθ cosφ/cosθ ω x ω y ω z (4) where Q is a projection atrix. Finally, fro equations (1), (3), and (4) we can write down the the entire odel of the quadcopter in a state space for as follows: ẋ = v x (5) ẏ = v y (6) ż = v z (7) v x = k d v x + kc ( )( ) sinψsinφ+cosψcosφsinθ v1 +v +v3 +v4 (8) v y = k d v y + kc ( )( ) cosφsinψsinθ cosψsinφ v1 +v +v3 +v4 (9) v z = k d v z g + kc ( )( ) cosθcosφ v1 +v +v3 +v4 (10) φ = ω x +ω y (sinφtanθ)+ω z (cosφtanθ) (11) θ = ω y cosφ ω z sinφ (1) ψ = sinφ cosθ ω y + cosφ ω x = Lkc I xx ( v 1 v 3 ω y = Lkc I yy ( v v 4 ω z = bc I zz ( v 1 v +v 3 v 4 cosθ ω z (13) ) ( ) Iyy I zz ω y ω z (14) I xx ) ( ) Izz I xx ω x ω z (15) I yy ) ( ) Ixx I yy ω x ω y. (16) Notice that in this odel we have not considered the ground effect. This eans that in principle we can have negative values for the z coordinate of the quadcopter. If we 5 I zz

6 Table 1: Paraeters of the Quadcopter odel Paraeter Sybol Value Unit Mass of the quadcopter 0.5 kg Radius of the quadcopter L 0.5 Propeller lift coefficient k Ns Propeller drag coefficient b Ns Acceleration of gravity g 9.81 /s Air friction coefficient k d 0.5 kg/s Quadcopter inertia about the x b -axis I xx kg Quadcopter inertia about the y b -axis I yy kg Quadcopter inertia about the z b -axis I zz 1 10 kg Motor constant c v s denote the state vector of the syste by x R 1 = [x y z v x v y v z φ θ ψ ω x ω y ω z ] T, and we define the input and output vectors as u R 4 = [v 1 v v 3 v 4] T and y R 6 = [x y z φ θ ψ ] T respectively, we can write down the odel of the quacopter in a ore copact way, ẋ = ξ(x,u) (17) y = Cx (18) where C R 6 1 is the output atrix and ξ is a nonlinear vector function defined by (5)- (16). Notice that the the input vector is in ters of the squared voltages of the rotors, and therefore your control syste should copute v 1, v v 3 and v 4 instead of v 1, v v 3 and v 4. The axiu voltage that can be applied to the otors is 10v. To ake sure that this constraint is not violated, the circuitry within the quadcopter incorporates a clipping echanis. Observe that negative voltages would change the spinning direction of the otors and therefore would invalid the odel that has been derived. Based on the previous considerations, it is clear that the input constraints of the syste are given by 0 v u 1,u,u 3,u v (19) where u 1 = v 1, u = v, u 3 = v 3 and u 4 = v 4. In the present setup, the attitude (φ,θ,ψ) of the quadcopter is coputed fro the readings of a gyroscope and an acceleroeter installed in the aircraft. The position (x, y, z) of the vehicle is deterined by a set of caeras installed within the roo. 3 Available tools In order to siplify the control design process, you have at your disposal the following tools: 6

7 A *.at file references xx.at (where xx corresponds to your group nuber) that contains the sequence of references for the x, y, and z coordinates of the quadcopter (references), the axiu siulation tie (Tax) and the initial state of the syste (x0 quadcopter). You have to load this file first before perforing any siulation with Siulink. You can always check which variables are in the workspace by typing the Matlab coand whos. A Siulink file teplate quadcopter.slx containing the nonlinear odel of the quadcopter and a block that iports the reference trajectory fro the workspace of Matlab. The input of the quadcopter block is a vector containing the squared voltages of the rotors (denoted by u(t)) and the outputs of this block are the entire state vector (denoted by x(t)) and the easured variables (denoted by y(t)), to which soe easureent noise has been added. A protected Matlab function generate report.p for visualizing and evaluating the siulation results after a Siulink run. It plots the trajectory followed by the quadcopter (blue line) within the roo as well as the checkpoints (red circles), the evolution in tie of the inputs or control actions (with indication of the input liits), the evolution of the angles ψ, θ and ψ of the quadcopter, and the evolution of the x, y and z coordinates together with their setpoints and axiu allowed deviations (0.08 ). This function also gives a siulation report indicating the nuber of checkpoints reached, and the tie that the aircraft has spent traveling fro one checkpoint to the next one. The quadcopter has a tie liit of 7 seconds to reach each checkpoint. For siulations where it is assued that the entire state vector is available (no observer), you should type in the coand window generate report(0). For the case where the state vector is estiated by an observer, you should type generate report(1). Rearks: Do not bother about the file quadcopter sfunction.p. It is used for ipleenting the nonlinear odel of the quadcopter in Siulink. Additionally, the file icon quad.png provides an iage ask for the Siulink block representing the aircraft. Make sure that the folder where you downloaded all the files is part of the Matlab path (or working directory). 4 Assignent 4.1 Linearization In order to design an LQR or an LQG controller, it is necessary to have a linear approxiation of the nonlinear odel around an operating point. In this exercise such operating point, which is also an equilibriu point, is the one of a stationary flight (the quadcopter is in hover). Deterine the linearization/equilibriu point of the vehicle for φ = θ = ψ = 0 and x = y = z = 0. 7

8 Linearize the nonlinear odel (5)-(16) about the equilibriu point. Do not forget to provide both the sybolic expression and the nuerical values of the atrices describing the resulting linear odel, ẋ = A x+b u y = C x where x = x x, u = u u and y = y y are the deviation variables and x, u and y are the values of x, u and y at the operating point. Note: It is always a good idea to verify how good your linear odel is in approxiating the nonlinear odel. To this end you can apply a sall step to the inputs of the odels and copare their respective outputs. We suggest you to apply the following step signal: t < 1, 0 u 1 = u = u 3 = u 4 = t 1, 5 and siulate both odels for 5 seconds. Do not forget to perfor the proper conversions between the deviation variables ( x, u, y) and the original variables (x, u, y) at the oent of using the odels. The previous test also serves to check whether you ake a istake or not when you derived the linear odel. 4. Discretization Choose a discretization rule (e.g., zero-order hold, Euler s rule, bilinear transforation - also called tustin s rule, etc.) and find a discrete-tie state-space odel for the quadcopter. Use a sapling tie of T s = 0.05 s. Provide the nuerical values of the atrices describing the discrete-tie odel. Is the discrete-tie odel stable?, what are the poles?, is it controllable?, is it observable?, is it stabilisable?, is it detectable?, is it inial?, are there transission zeros? Reark: The discrete-tie odel derived here, is the odel that you have to use for designing the control schees of the following subsections. 4.3 LQR control The control goal is to drive the quadcopter to several checkpoints as fast as possible (tracking proble). Furtherore, the control syste should be capable of driving the aircraft to the checkpoints when carrying a payload of 0.1 kg. A checkpoint is declared reached when the quadcopter enters a sphere around it. The radius of this sphere, which is 0.08, is the axiu aditted error. The quadcopter is confined to a roo that is 6 eters wide, 6 eters long, and 3 eters high. In this section it is assued that the entire state-vector is available. So, use the proper output of the quadcoper block in Siulink. Do not forget that for generating a siulation report, you have to type in the coand window generate report(0) after a Siulink siulation run. 8

9 Experients without payload. Initially ake sure that the payload paraeter of the quadcopter block is set to 0 kg (to access the paraeters window double-click the quadcopter block). Ipleent a full-state feedback controller (N x, N u, K) based on an LQR kind of feedback. Ipleent an LQR controller with integral action. Keep in ind that we are interested in tracking only the x, y and z coordinates of the vehicle. So, set the nuber of integrators accordingly. Verify the controllability of the augented syste. Briefly describe the design process, indicating how you obtained the weighting atrices for both control schees. Discuss the siulation results. Which control strategy perfors better? Experients with payload. Adding a payload or cargo to the quadcopter can be seen as a disturbance that affects the dynaics of the aircraft. Now, ake sure that the payload paraeter of the quadcopter block is set to 0.1 kg. Keep the weighting atrices previously found, and siulate again both control systes. What is the effect of the payload on the perforance of the controllers? which control syste is ore robust? Discuss. You can change the tuning of your controllers if they becoe unstable. What are the fundaental differences between the full-state feedback controller and the integral controller? Reark: It is extreely iportant to set the saple tie property of the Siulink blocks (gains, discrete-tie blocks, etc.) that are part of your controller to T s. 4.4 LQG control For this section you have to use the LQR controller with integral action that you designed in Section 4.3. Additionally, the assuption that the entire state-vector is available is NOT valid anyore. Therefore your control syste only has at its disposal the easureents given by the sensors. Do not forget that for generating a siulation report, you have to type in the coand window generate report(1) after a Siulink siulation run. Design and ipleent a Kalan Filter for estiating the entire state-vector. The the variance of the easureent noise for x,y,z is and for φ,θ,ψ is Briefly discuss how you fixed the covariance atrix of the process noise and the covariance atrix of the easureent noise. Note: In the discretetie stochastic odel of the syste (see page 174 of the course notes), assue B 1 = I R 1 1, the identity atrix. Carry out a siulation with a payload of 0 kg. What is the effect of adding a Kalan filter on the overall perforance of the control syste? Discuss the results. 9

10 Perfor a siulation with a payload of 0.1 kg, and copare the results with the ones obtained in the previous section. Discuss. 5 Deliverables We expect fro you: A written report (a hard copy and a digital copy). All the Siulink and Matlab files that allow us to reproduce your results. Also provide a docuent where you briefly explain the purpose of each of the. Rearks about the report: The report should be written preferably in English, and it should address every single point of Section 4. Your report should describe the technical aspects of the designed controllers, and clearly it should state which design specifications were et and which were not (and why they could not be achieved). Not only the technical correctness of your report is evaluated but also the quality and presentation. So, ake sure that everything is clear and well explained. For exaple, ake sure that you label every axis of every plot, that your figures include legends if necessary, that in the text you discuss or address what is presented in every figure of the anuscript, that the text is free of typos and well redacted, etc. Do not forget to include the Siulink diagras that you have created. If a Siulink diagra contains subsystes, you should also present the contents of these subsystes (excluding the Siulink blocks that were given to you). In addition, your report ust be self-contained. Do not refer the reader to your Matlab files. Keep in ind that we only ask you for your Matlab files for verification purposes. 6 Exaination At the end of the seester you will have an oral exa, where you will have to defend your design choices, explain technical details, answer soe general questions, etc. Further inforation (dates of exaination, how the points are distributed between exercises and practicu, etc.) can be found in the webpage of the Coputergestuurde Regeltechniek course in Toledo. 10