CONTROL DESIGN FOR UNMANNED AERIAL VEHICLES WITH FOUR ROTORS

Transcription

1 Interdisciplinary Description of Coplex Systes 14(2), , 2016 CONTROL DESIGN FOR UNMANNED AERIAL VEHICLES WITH FOUR ROTORS Denis Kotarski 1, *, Zoran Benić 2 and Matija Krznar 3 1 Karlovac University of Applied Sciences 1 Karlovac, Croatia 2 Končar D&ST 2 Zagreb, Croatia 3 Peti Brod 2 Zagreb, Croatia DOI: /indecs Regular article Received: 7 February Accepted: 9 March ABSTRACT Altitude and attitude controlled quadcopter odel is used for the behavior and control algorith testing, before the ipleentation on the experiental setup. The ain objective is the control syste design which will achieve good task perforance in the cobination with the quadcopter dynaic odel. Also, for the control odel, it is iportant to be relatively easy to odify for the use of other control algoriths and to be easy to ipleent on the experiental setup. At the beginning of this article, the control syste design process is described. Quadcopter dynaics equations are siplified by applying several assuptions and in that for are used for the controller synthesis. Quadcopter control syste is split into inner and outer control loop because the quadcopter is underactuated syste which eans that the direct control of all of the degrees of freedo is not possible. In the second part, the PI-D control algorith is described which is applied on the siplified quadcopter dynaic odel. The inner loop controls roll, pitch and yaw angles together with the quadcopter altitude. Its outputs are four control variables. Outer loop controls quadcopter X and Y position. Its outputs are roll and pitch desired angles. Regulated quadcopter odel behavior is shown for the three types of task. First, the achieving of position in space is siulated. Second, the reference trajectory tracking is shown. Last task shown is the reference trajectory tracking with added periodical disturbances. Siulations show bounded positions error of the regulated quadcopter syste using PI- D controller for the different types of tasks perfored under different conditions. KEY WORDS quadcopter, control design, inner control loop, outer control loop, PI-D controller CLASSIFICATION JEL: Z19 PACS: Tp, Iu *Corresponding author, : denis.kotarski@triviusci.co; ; *Koor Začretski 30, HR Sveti Križ Začretje, Croatia

2 Control design for unanned aerial vehicles with four rotors INTRODUCTION In order for the quadcopter to be able to execute tasks, control algoriths are needed to be ipleented. The goal of the control syste design is stabilization of the syste state around the desired reference state, or changing that state according to soe law, regardless of the inner or outer disturbances. That is achieved with the feedback, which allows coparison of the easured value of the process with the desired one. Prior the control algorith ipleentation on the experiental setup, it ust be tested in cobination with the dynaic odel [1]. Quadcopter has six degrees of freedo (6 DOF) and four control variables which eans that the quadcopter is underactuated and dynaically unstable syste. The only oving parts are the propulsor propellers which are rotating with respect to the fixed propulsor axis. Quadcopter frae has a cross (+) configuration where X B and Y B axes go through the propulsors. X B and Y B are body coordinate frae axes (body coordinate frae (syste) F B is fixed to the quadcopter body (frae)). Quadcopter frae is assued to be a rigid structure. The only variable thing is considered to be the propellers revolutions per inute (RPM) that influence the quadcopter oveent. CONTROL SYSTEM DESIGN Quadcopter dynaics is derived fro the quadcopter dynaic odel [2]. The ain equations are quadcopter accelerations as functions of the control variables: X = (cosψsinθcosφ + sinψsinφ) U 1 (1) Y = (sinψsinθcosφ cosψsinφ) U 1 Z = g + (cosθcosφ) U 1 p = I yy I zz I xx q = I zz I xx I yy (2) (3) qr J TP I xx qω + U 2 I xx (4) pr + J TP I yy pω + U 3 I yy (5) r = I xx I yy I zz pq + U 4 I zz, (6) where X, Y, Z are acceleration in the earth fixed coordinate frae (F E ). φ, θ, ψ are roll, pitch and yaw angles with respect to F E. U 1, U 2, U 3, U 4 are control variables. is quadcopter ass, g is gravitational acceleration. I xx, I yy, I zz are oents of inertia with respect to F B. J TP is the propulsors rotor and propeller oent of inertia. Ω is overall propeller speed. p, q, r are angular velocities with respect to F B, while p, q, r are angular accelerations with respect to F B. Control variables are a function of squared angular velocities of the propellers: U 1 = b(ω ω ω ω 4 2 ) (7) U 2 = lb( ω ω 4 2 ) (8) U 3 = lb( ω ω 3 2 ) (9) U 4 = d( ω ω 2 2 ω ω 4 2 ) (10) Ω = ω 1 + ω 2 ω 3 + ω 4, (11) 237

3 D. Kotarski, Z. Benić and M. Krznar where l is the distance between the quadcopters center of gravity and propulsors axis, b is the thrust coefficient, d is the drag coefficient, ω i is angular velocity of the propeller i. The objective of quadcopter stabilization is finding the values of the propellers angular velocities in a way that keeps the quadcopter in desired position and attitude. This is done by the eans of the inverse kineatics, i.e. inverse dynaics. Unlike the direct proble, inverse operations are not always possible and are not always unique. Quadcopter dynaics has to be siplified in order to get siple inverse odel that can be easy ipleented in control algoriths. Equations (1)-(6) have to be siplified with keeping in ind the following assuptions: angular contributions to the accelerations are quite coplex because they depend on a couple of paraeters, ost of which are a vector product of angular velocities. Keeping in ind that the quadcopter oveent is near hover state, there are very little changes in angles, therefor those contributions can be ignored. angular accelerations are linked with the change of angles with respect to F E. They are not equal to the accelerations of the Euler angles in F B. Transforation atrix T defines the relation between the angular velocities with respect to F E and the ones in F B. Quadcopter oveent being close to hovering state, T is close to the identity atrix, aking the angular acceleration equations the sae in F E and F B. control algorith gives the control signals to the propulsors. With four control variables, in a single loop, it is not possible to regulate ore than four DOF. Inner loop controls the altitude and attitude, while the outer loop controls quadcopter position by giving the attitude references to the inner loop. Quadcopter dynaics equations that are used in control synthesis are: Z = g + (cosθcosφ) U 1 (12) φ = U 2 I xx (13) θ = U 3 I yy (14) ψ = U 4 I zz, (15) where φ, θ, ψ are angular acceleration with respect to F E. Control algorith as inputs has sensor data (or calculated data fro the dynaic odel) and reference data. Outputs of control algorith are the angular velocities of four propellers, which can be, through dynaic odel, translated to the pulse-width odulation (PWM) signal. Quadcopter, being an underactuated syste, does not allow direct control of all of the DOFs. Control of positions X and Y is achieved indirectly through control of roll and pitch angles in outer control loop. Quadcopter cannot ove translationally without the rotation around one of the quadcopter axes. Outer control loop analyses sensor data and desired task data, and its outputs are desired angle values for the inner control loop which then controls the roll, pitch and yaw angles. Inner control loop is the core of the control algorith. It analyses sensor and outer control loop data together with desired altitude task data. Its outputs are control variables which stabilize the position error through four basic quadcopter oveents. Fro the equations (7) (10) control variables can be retrieved for the desired accelerations. PI-D controller is used for the desired accelerations assessent. Inverted oveent atrix is used for the calculation of the squared angular propeller velocities for four control variables: 238

4 Control design for unanned aerial vehicles with four rotors TASK X des Y des Z des ψ des CONTROL ALGORITHM OUTER CONTROL LOOP ϕ des 2 θ INNER U INVERTED ω des i i CONTROL MOVEMENT LOOP MATRIX QUADCOPTER DYNAMICS X es Y es SENSORS Z es ϕ es θ es ψ es PI-D CONTROLLER Figure 1. Quadcopter control diagra. ω 1 2 = 1 4b U 1 1 2bl U 3 1 4d U 4 (16) ω 2 2 = 1 4b U 1 1 2bl U d U 4 (17) ω 3 2 = 1 4b U bl U 3 1 4d U 4 (18) ω 4 2 = 1 4b U bl U d U 4. (19) Classic PID structure has a derivative ter calculated fro the error signal, which is a downside of that structure when it coes to the quadcopter control. If the task takes the for of the step function, output of the derivator will be an ipulse which could bring the propulsors to the state of saturation and push the syste outside of the linear space. That is the reason for the PI-D structure of the controller (Figure 2) in which the derivative ter is being calculated fro the sensor data. 1 s K I TASK + e K + + u P - - SENSORS s (ateatical odel) K D Figure 2. PI-D controller structure. 239

5 D. Kotarski, Z. Benić and M. Krznar INNER CONTROL LOOP Inner control loop controls quadcopter altitude and attitude. Input variables for inner loop can be divided in two parts, task and sensor variables as shown in figure 2. Task variables are desired altitude and yaw angle that are given by the task that quadcopter needs to accoplish and desired roll and pitch angles, that are calculated in the outer control loop. Sensors give easured altitude, roll angle, pitch angle and yaw angle. Inner loop outputs are four control variables. Altitude control Equation for the thrust force control variable U 1 is: U 1 = K PZ e Z + K IZ e Z K DZ Z es + g, (20) where K PZ, K IZ and K DZ are three altitude PI-D controller paraeters. e Z is the altitude error, where e Z = Z des Z es. Z des is the desired altitude and Z es is the easured altitude. Roll control Equation for the roll oent control variable U 2 is: U 2 = K Pφ e φ + K Iφ e φ K Dφ φ es, (21) where K Pφ, K Iφ and K Dφ are three roll angle PI-D controller paraeters. e φ is the roll angle error, where e φ = φ des φ es. φ des is the desired roll angle and φ es is the easured roll angle. Pitch control Equation for the pitch oent control variable U 3 is: U 3 = K Pθ e θ + K Iθ e θ K Dθ θ es. (22) Siilar to the roll control, K Pθ, K Iθ and K Dθ are three pitch angle PI-D controller paraeters. e θ is the pitch angle error, where e θ = θ des θ es. θ des is the desired pitch angle and θ es is the easured pitch angle. Yaw control Equation for the yaw oent control variable U 4 is: U 4 = K Pψ e ψ + K Iψ e ψ K Dψ ψ es, (23) where K Pψ, K Iψ and K Dψ are three yaw angle PI-D controller paraeters. e ψ is the yaw angle error, where e ψ = ψ des ψ es. ψ des is the desired yaw angle and ψ es is the easured yaw angle. OUTER CONTROL LOOP Outer control loop is used because the quadcopter is underactuated syste and it is not possible to control all of the quadcopter DOF directly. As said before, the inner loop directly controls 4 DOF, those being three angles and altitude. To be able to indirectly control X E and Y E position, outer loop is used. Outer control loop, as its outputs, give desired roll and pitch angles to the inner loop for the desired X E and Y E position. Outer control loop is superior to the inner control loop. Equations (1) and (2) can be singled out which are the equations for the quadcopter X E and Y E linear accelerations: 240

6 Control design for unanned aerial vehicles with four rotors X = (cosψsinθcosφ + sinψsinφ) U 1 (24) Y = (sinψsinθcosφ cosψsinφ) U 1. (25) Quadcopter dynaics of the X E and Y E linear accelerations can be siplified in the siilar way as the quadcopter dynaics equations (12) (15). Again, the quadcopter oveent is observed as a hovering state, i.e. roll and pitch angles are very sall (close to 0). Keeping that in ind, the equations (24) and (25) can be transfored to: X = (cosψ θ + sinψ φ) U 1 (26) Y = (sinψ θ cosψ φ) U 1. (27) Matrix notation of the equations (26) and (27): [ X ] = U 1 Y [ sinψ cosψ cosψ sinψ ] [φ ]. (28) θ Fro the equation (28), roll and pitch angles are being calculated. Because of the assuption that the quadcopter oveent is near the hovering state, thrust force U 1 has to nullify the gravitational force allowing for the U 1 / to be changed with the gravitational acceleration g. Equation for the desired roll and pitch angles is: [ φ des ] = 1 θ des g [sinψ cosψ cosψ sinψ ] [X ]. (29) Y SIMULATION RESULTS Behavior of the controlled quadcopter syste depends on the dynaic odel of the quadcopter and the control algorith. Control algorith input values are the values defined in the task as well as the dynaic odel outputs. Control algorith outputs are the four control variables which are also the dynaic odel inputs. TASK CONTROL ALGORITHM MATHEMATICAL MODEL OF QUADCOPTER Figure 3. Controlled quadcopter syste. Siulation results are given for the three cases. First case is the siulation of the achieving the desired position. Second case is the reference trajectory tracking and the final case is the reference trajectory tracking with the addition of the periodic disturbance. ACHIEVING THE DESIRED POSITION The desired point in space, defined by three coordinates with respect to F E, is arbitrarily chosen. The control algorith is tuned for the quickest desired point achieveent. Siulation tie is 20 s. 241

7 D. Kotarski, Z. Benić and M. Krznar Figure 4. Angular propeller velocities achieving the desired position in space. Figure 5 shows the quadcopter attitude which constantly changes until the quadcopter gets to the desired position. After achieving that, roll and pitch angles are 0. Yaw angle is arbitrarily chosen. Figure 5. Quadcopter attitude achieving the desired position in space. Figure 6 shows that after certain aount of tie (cca. 10 sec), depending of the controller paraeters, quadcopter reaches the desired position (X E = 15, Y E = 6, Z E = 10) and holds that state. Figure 6. Quadcopter position achieving the desired position in space. REFERENCE TRAJECTORY TRACKING Chosen trajectory is of a helix shape because of the siple generating and displaying. Helix is a cobination of three functions, two of which are of sinusoidal shape ( X E and Y E position), and one is linear (Z E position). Siulation tie is set to 100 s. 242

8 Control design for unanned aerial vehicles with four rotors Figure 7. Propeller angular velocities reference trajectory tracking. Figure 7 shows propeller angular velocities which depend on the reference trajectory. Figure 8. Quadcopter attitude reference trajectory tracking. Figure 9 shows that quadcopter quickly gets close to the reference trajectory and that is tracking it with bounded tracking error, being unable to achieve asyptotic error convergence due to the variable reference state. Figure 9. 3D view of reference trajectory tracking. REFERENCE TRAJECTORY TRACKING WITH ADDED PERIODICAL DISTURBANCES In this case of trajectory tracking, periodical external force disturbances are added in XY plane. X coordinate disturbance profile have aplitude of 2 N, period of 10 seconds, duration of 0,5 second and phase delay of 8 seconds. Y coordinate disturbance profile have aplitude of 1,5 N, period of 15 seconds, duration of 1 second and phase delay of 6 seconds. 243

9 D. Kotarski, Z. Benić and M. Krznar Figure 10. Propeller angular velocities reference trajectory tracking with added periodical disturbances. Figure 11 shows changes of angular velocities which depend on the reference trajectory and given periodical disturbances. Figure 11. Quadcopter attitude reference trajectory tracking with added periodical disturbances. Figure 12 shows quadcopter following the reference trajectory with bounded position error. Disturbances are affecting the quadcopter position and attitude, but in the acceptable boundaries which are not allowing the quadcopter to lose track of the reference trajectory. Figure 12. 3D view of reference trajectory tracking with added periodical disturbances. CONCLUSIONS This article has shown control design and control algorith ipleentation which in the cobination with the quadcopter dynaic odel for regulated quadcopter syste. During 244

10 Control design for unanned aerial vehicles with four rotors the control design phase, soe assuptions were ade in order to get siplified quadcopter dynaics equations. Quadcopter behavior while perforing different tasks in different conditions was shown by siulation where soe control variables constraints were ade with the goal of ore realistic description of quadcopter behavior. Controller paraeters where chosen epirically. Outer control loop outputs are inner control loop inputs so the outer loop paraeters were chosen in a way that in the cobination with the inner loop paraeters give the ost suitable control variables. Siulation results have shown the stability, robustness and error boundedness of the PI-D controller. Further work will include control algorith ipleentation on the experiental setup. Also, ore advanced control algoriths will be tested with the objectives to iprove quadcopter syste robustness and tracking perforances as well as a better energy anageent. REFERENCES [1] Bresciano, T.: Modelling, Identification and Control of a Quadrotor Helicopter. M.Sc. Thesis. Lund University Departent of Autoatic Control, Lund, 2008, [2] Benić, Z.; Piljek, P. and Kotarski, D.: Matheatical odelling of unanned aerial vehicles with four rotors. Interdisciplinary Description of Coplex Systes 14(1), , 2016, [3] Luukkonen, T.: Modelling and Control of a Quadcopter. Mat , School of Science, Espoo, 2011, [4] Magnussen, Ø. and SkjØnhaug K.E.: Modelling, Design and Experiental Study for a Quadcopter Syste Construction. University of Agder Faculty of Technology and Science, Kristiansand, 2011, [5] Beard, R.W.: Quadrotor Dynaics and Control. Brigha Young University, Provo, 2008, [6] Raza, S.A.: Intelligent Flight Control of an Autonoous Quadrotor. In: Casolo, F., ed.: Motion Control. University of Ottawa, Ottawa, p.580, 2010, [7] Kasać, J.; Stevanović, S.; Žilić, T. and Stepanić, J.: Robust Output Tracking Control of a Quadrotor in the Presence of External Disturbances. Transactions of FAMENA 37(4), 29-42, 2013, [8] Naidoo, Y.; Stopforth, R. and Bright, G.: Quad-Rotor Unanned Aerial Vehicle Helicopter Modelling & Control. International Journal of Advanced Robotic Systes 8(4), , 2011, [9] Choi, Y.C. and Ahn, H.S.: Nonlinear Control of Quadrotor for Point Tracking: Actual Ipleentation and Experiental Tests. IEEE/ASME Transactions of Mechatronics 20(3), , 2015, [10] Magsino, E.R. et al.: Stabilizing Quadrotor Altitude and Attitude through Speed and Torque Control of BLDC Motors. 13 th International Conference on Control, Autoation, Robotics & Vision, IEEE, 2014, 245