Control strategies evaluation for stabilizing a quadcopter docking on a side wall.

(1)

IN

DEGREE PROJECT MECHANICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2019,

Control strategies evaluation for stabilizing a quadcopter docking on a side wall

EDUARDO FUENTEVILLA BLANCO

KTH ROYAL INSTITUTE OF TECHNOLOGY

SCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT

(2)

(3)

Sammanfattning

Denna avhandling överväger stabilisering av en quadcopter i fysisk kontakt med en sidovägg genom en mekanisk arm. Dronen accelereras med sikte på att tillämpa en större kraft mot väggen. Följaktligen adopterar dronen ett lutat läge som måste styras. Tre kontrollstrategier;

Proportional-integral-derivat (PID) kontroll, glidstyrningskontroll (SMC) och modell prediktiv kontroll (MPC) tillsammans med två olika arm design är iterated till uppnå det snabbaste och stabila tillvägagångssättet. För det första en avledning av drondynamiken för varje arm är uppnådd. För det andra implementeras de olika kontrollstrategierna baserat på det härledda dynamik. De erhållna resultaten visar att dronen är kapabel att docka i en viss hastighet, Återstående stabila samtidigt som man utövar en kraft mot väggen.

2

(4)

(5)

Abstract

This thesis contemplates the stabilization of a quadcopter in physical contact with a side wall via a mechanical arm. The drone is accelerated aiming to apply a greater force against the wall. Consequently, the drone adopts a tilted position that must be controlled. Three control strategies; Proportional-integral-derivative (PID) control, sliding mode control (SMC) and model predictive control (MPC) together with two different arm designs are iterated to achieve the fastest and stable approach. Firstly, a derivation of the drone dynamics for each arm is achieved. Secondly, the different control strategies are implemented based on the derived dynamics. The obtained results show that the drone is capable of docking at a certain speed, remaining stable while exerting a force against the wall.

4

(6)

(7)

Acknowledgements

I would like to acknowledge my colleagues and friends from my time being in Stockholm. Those days of hard work and joy together will be always in my memories. I would also like to mention the KTH Royal Institute of technology together with their staff whose provided education has broaden my knowledge in fields that I love and enjoy.

In addition, thanks to my family who have been there for me as well as I always be there for them. Thanks to their support and motivation to keep going. Finally, special mention to my father who convinced me to follow what I really wanted to do and Maria Ruiz Gonzalez who has been the most important support both emotionally and motivationally.

Eduardo Fuentevilla Blanco 22/05/2019

6

(8)

(9)

Abbreviations

3D Three Dimensional

CAD Computer-Aided Design

CFRP Carbon Fiber Reinforced Polymer

COM Center Of Mass

EDS Equivalent Plastic Strain

IMU Inertial Measurement Unit

MIMO Multi-Input Multi-Output

MPC Model Predictive Control

PID Proportional Integral Derivative

QP Quadratic Program

SI International System

SMC Sliding Mode Control

UAV Unmanned Aerial Vehicle

VR Virtual Reality

8

(10)

Definitions

The following definitions are explained as being considered helpful for the reader understanding.

Figure 0.0.1: Roll, pitch and yaw definition.

• Attitude: Refers to the drone orientation in terms of three components: roll, pitch and yaw.

• Chattering: Common problem in sliding mode control application consisting of undesirable phenomenon of oscillations having finite frequency and amplitude. It leads to low control accuracy, high wear of moving mechanical parts, and high heat losses in power circuits.

• Docking: Refers at the drone maintaining continuous contact with a side wall.

• Docking pitch: Pitch at which the contact with the wall is achieved.

• Euler angles: Three angles to describe the orientation of a rigid body respect to a fixed coordinate system. For an UAV those angles are roll, pitch and yaw.

• Final pitch: Further pitch obtained on results section after achieving docking. It corresponds to the maximum stable pitch.

9

(11)

List of Figures

0.0.1 Roll, pitch and yaw definition . . . 9

1.1.1 UAV concept design . . . 17

1.5.1 Mechanical arm designs. . . 19

2.0.1 Climbing robot example . . . 21

3.0.1 Overall simulation . . . 24

3.1.1 Quadcopter overall parameters . . . 25

3.1.2 Quadcopter with arm overall parameters . . . 28

3.1.3 Overall parameters including arm while docking . . . 30

3.2.1 FCS block inside . . . 32

3.2.2 Controller block inside . . . 33

3.2.3 Docking block inside . . . 33

3.2.4 Control idea . . . 34

3.2.5 Control mixer block inside . . . 35

3.2.6 PID block . . . 36

3.2.7 PID altitude controller . . . 41

3.2.8 PID pitch/roll controller . . . 42

3.2.9 PID yaw controller . . . 42

3.2.10SMC block . . . 43

3.2.11MPC block . . . 46

3.2.12Prediction horizon comparison . . . 50

3.2.13Control horizon comparison . . . 50

3.2.14MPC controller . . . 51

4.1.1 Euler angles with single-support arm using PID control . . . 54

4.1.2 Force generated against the wall with single-support arm using PID control . . 54

4.1.3 Euler angles with three-support arm using PID control . . . 55

4.1.4 Force generated against the wall with three-support arm using PID control . . . 55

4.1.5 PID Control signals single contact arm . . . 56

4.1.6 PID Control signals three contacts arm . . . 56

4.2.1 Euler angles with single-support arm using SMC . . . 57

4.2.2 Force generated against the wall with single-support arm using SMC . . . 57

4.2.3 Euler angles with three-support arm using SMC . . . 58

4.2.4 Force generated against the wall with three-support arm using SMC . . . 58

4.2.5 SMC Control signals single contact arm . . . 59

4.2.6 SMC Control signals three contacts arm . . . 59 12

(14)

4.3.1 Euler angles with single-support arm using MPC . . . 60

4.3.2 Force generated against the wall with single-support arm using MPC . . . 60

4.3.3 Euler angles with three-support arm using MPC . . . 61

4.3.4 Force generated against the wall with three-support arm using MPC . . . 61

4.3.5 MPC Control signals single contact arm . . . 62

4.3.6 MPC Control signals three contacts arm . . . 62

4.4.1 Control strategies performance comparison . . . 63

4.4.2 Control strategies performance comparison . . . 63

6.0.1 Quadcopter movement dependency on rotors thrust. . . 75

6.0.2 Stress and EPS analysis single contact arm. . . 79

6.0.3 Stress and EPS analysis single contact arm for 5° deviation. . . 80

6.0.4 Stress and EPS analysis single contact arm for 10° deviation. . . 80

6.0.5 Stress and EPS analysis three contacts arm. . . 81

6.0.6 Stress and EPS analysis three contacts arm for 355° deviation. . . 82

6.0.7 Stress and EPS analysis three contacts arm for 5° deviation. . . 82

13

(15)

List of Tables

3.2.1 PID table values both arms . . . 41

3.2.2 SMC table values both arms . . . 45

3.2.3 MPC table values both arms . . . 49

4.4.1 Comparison table both arms . . . 64

14

(16)

(17)

Chapter 1 Introduction

This thesis proposes the use of a quadcopter capable of docking on a side wall for applications where applying a force is required. A new stability challenge arises; control the drone altitude and attitude while a constant force is exerted against the wall. The whole dynamic system composed of quadcopter and arm needs to find an equilibrium considering the reaction force due to contacting the wall while keeping a fixed altitude. The variety, accessibility and stability that quadcopters present comparing with other multicopters are what motivate me to choose this type of four rotors unmanned aerial vehicle (UAV).

1.1 Background

UAVs or in other words, aircrafts without human pilot aboard, have been massively used over the past decades and still do so. They allow operators reaching confined or inaccessible places where human intervention is required. Among many other applications, filming, defense or construction planning are the most exploited nowadays. The capacity of drones to carry a camera weight permits to record in any location in a 3D space environment. Not only for film productions but also for maintenance surveillance on structures such as building or bridges.

Military applications have always been priority when it comes to the use of new technologies, thus, UAVs are not an exception. Counter terrorism, espionage and exploration operations have taken advantage of them. The capability of drone to fly several meters above the ground is of heavy use for construction planning. Builders collect real-time data to improve organization and reduce delivery time of their projects by using drones.

However, there are still some applications which have not been mastered, yet to be developed. In particular, thickness measurement surveillance. Industries such as energy, chemical or transport require a regular thickness inspection of their piping, vessels, hulls or deposits in order to avoid any leakage that can end up in dramatic consequences. Environmental pollution, workplace accidents and economic losses among others catastrophic events.

As of now, this type of surveillance is performed by certified operators. The process consists of contacting the desired surface with a measuring probe, emitter of ultrasonic waves. The thickness is calculated by measuring how long it takes for a sound pulse generated by the probe to be reflected and come back again. A minimum force as well as a perfect alignment of the probe with respect to the surface is required for the measurement samples to be precise and therefore valid. Furthermore, unlike other conventional methods such as caliper or micrometers, it only requires access to one side of the material. However, this technique suppose a risk on operators’safety when those are carried out in high or confined areas.

16

(18)

Figure 1.1.1: UAV concept design.

A new alternative arises with the existence of UAVs. The capability of UAVs to physically interact with the environment is a trending and under research topic nowadays [1]. A well controlled drone is able to physically dock on walls. Together with the addition of the above mentioned probe and a fairly amount of force, the thickness could be successfully measured.

This achievement would improve this type of surveillance as it is often required in high or confined areas with difficult accessibility for humans. Consequently, a reduction on inspection time due to drones rapidness and pass through anywhere skills. It would also suppose the end of operators performing such time-consuming, tedious and dangerous task. Nevertheless, the drone control has to be so precise that the UAV will never lead to any damage by bumping or crashing into the structures.

Although the non-destructive testing of structures using ultrasounds is the main source of inspiration to accomplish this research, other applications like painting, cleaning or even pushing large objects among others can benefit from it.

1.2 Purpose

The objective of this thesis is to simulate a new alternative to apply a specific force on side walls using a quadcopter. To be able to exert a force, the drone must adopt a tilted position such that the thrust generated by the propellers is pointing towards the wall. The resultant force consists of two components; one to keep the drone’s altitude and the other to generate the already mentioned pushing force. The stabilization of the drone together with an arm in physical contact with a side wall is simulated.

As an alternative aim, the possibility of quadcopters to accomplish their duties faster is also explored for the sake of inspection time reduction. Consequently, maximizing the battery

17

(19)

life usage. Different velocities at which the drone docks are iterated in order to verify if the stabilization is still possible.

To sum up, the end effect purpose is to come up with an improved alternative to humans for the non-destructive ultrasonic thickness measurement.

1.3 Research question

The problems this thesis aims to solve are summarized with the following research question:

• What control strategy and mechanical design provide a more stable docking on a side wall of a quadcopter approaching at a certain velocity?

To answer this research question, a simulation of a quadcopter hybrid model is proposed. Three control strategies together with two different mechanical arms are assessed.

1.4 Delimitations

The thesis is limited to a computer simulation rather than a verification with a real drone.

The quadcopter used as a reference is the Intel® Aero Ready to Fly Drone [2]. Parameters such as inertia, weight or center of mass (COM) are based on it [3]. Regarding the drone dynamics, its derivation is based on rigid body theory; no deformation or flexion of the quadcopter is considered. A hybrid model consisting of two control modes is described:

• Free flight mode: Drone flying with no contact with the environment. Its control strategy is based on a PID control, no other control strategies are contemplated as considered out of the scope.

• Docking mode: Drone in permanent contact with a side wall. Three control strategies are evaluated on this mode:

– Proportional-integral-derivative control.

– Sliding mode control.

– Model predictive control.

Not only are three control strategies simulated but also two different mechanical arm designs mounted on the front. Environmental conditions such as air pressure, temperature and magnetic field influence are neglected due to its low impact on the dynamics. The sensor imperfections are also taken into account. Consequently, the control loop returns the measured states with an error. The air flow disturbance is neglected taking into consideration the arm length. The air flow field influence due to the drone proximity to walls can be ignored as long as the distance between the rotor tip and the side wall is greater than 1.5 times the diameter of the rotor [4].

18

(20)

1.5 Method

The dynamics are derived by applying rigid body theory. The frame strain is neglected because of high stiffness materials (carbon fiber reinforced polymer (CFRP) and stainless steel) as well as its adequate structural design. Once the dynamics are obtained, the PID control, SMC and MPC strategies are derived. Regarding the mechanical arms, an arm with a single support on the wall is first considered followed by a three-support arm. The idea is to verify which arm design achieves the docking stabilization and which one does it in a more stable way. As a special characteristic, this arm is always facing the target wall by being oriented 90 degrees respect to this, independently of the quadcopter attitude.

(a) Single-support arm. (b) Three-support arm.

Figure 1.5.1: Mechanical arm designs.

The control strategies together with the dynamics obtained are simulated using some well known software tools; Matlab and Simulink. The scenario to be simulated is a drone flying towards and docking a side wall. Due to its hybrid nature, different dynamics are applied depending on whether the drone is free flying or docked on the wall. Furthermore, several contacting velocities are simulated in order to identify the maximum velocity at which the stabilization is accomplished. The arm is modelled with a spring-damper mechanism mounted on the tip, to damp the disturbances due to the contact. To optimize the quadcopter mechanical design, a CAD model is implemented using Autodesk Inventor. Moreover, this helps to determine parameters such as weight, inertia or COM of the complete system; quadcopter with arm. The thesis starts from a template provided by Mathworks where a quadcopter for free-flight and trajectory planning purposes is implemented [5].

19

(21)

(22)

Chapter 2 Frame of reference

Robots others than UAVs, like climbing ones, are capable of doing the same labour but in a more inefficient way. Human intervention is required; if a surface located in a non-consecutive place wants to be inspected, they must be placed manually. It is the case of [6] and [7] which create attraction forces through vacuum or [8] which uses the Bernoulli-effect for the same goal.

Figure 2.0.1: Climbing robot example.

As of now, docking on a wall has already been achieved successfully one way or another, using different kinds of UAVs, control strategies and mechanical designs. In [9] contacting with the environment is achieved by using a ducted-fan miniature UAV. The purpose is the realization of tasks such as inspection of buildings or manipulation of objects. No special contact mechanism is designed, the UAV’s body itself is the contacting agent. A hybrid automaton [10]

is implemented, which describes different behaviours of the system when free flying and when interacting with the environment. In [11] a mechanical manipulator system is mounted on a quadcopter for contacting with the environment. The mobility of the manipulator allows the drone to dock more easily, since this adapts to the approaching drones attitude. A coaxial-rotor unmanned helicopter is used in [12] for the same purpose. A hybrid modelling approach is followed to represent the dynamics and a model predictive controller to control the UAV. Not

21

(23)

only docking but sliding is accomplished in this research. A lightweight docking mechanism is developed to provide damping during contact and mechanically aid the docking manoeuvre. In [13] using an unmanned tri-tiltrotor, the capacity of pushing large objects is accomplished. The proposed control strategy is a Gain-scheduled PID-double derivative. The rotors are the ones which tilt and not the drone itself to apply the force. Finally, in [14], a tricopter is used with the aim of achieving a non-destructive ultrasonic and corrosion inspection. Its main characteristic is the use of electromagnets to stick onto the wall. Nevertheless, this solution is only valid for ferromagnetic surfaces.

The work in [13] shares more similarities with this thesis, as both focus on applying a force while docking. However, this thesis solution must overcome some extra challenges. The capability of the rotors to tilt is neglected on the UAV chosen. Consequently, it is the drone itself which must tilt (pitch) in order to generate a force of greater magnitude. This causes a complex stability problem; stabilize a drone in a tilted position while being in contact with a side wall. It requires to control the force generated by the propellers such as the drone produces a constant force while keeping stability. The whole dynamic system must find an equilibrium taking into account the reaction force due to contacting the wall while maintaining a fixed altitude [13].

Furthermore, since the drone is tilted, a disturbed air flow is generated due to the propellers pointing towards the wall or structure. However, this can be neglected if the distance between the rotor tip and the side wall is greater than 1.5 times the diameter of the rotor as stated in [4].

To add on, the short flight time of UAVs, an average of 25 min, motivates researchers to discover and analyse ways of improving the battery life and drone efficiency flight. This is the case of [15] which proposes a fully automatic charging station capable of operating wirelessly or [16] which explores the development of an energy model that takes into account wind direction, wind speed and payload to decide the most energy efficient path for delivery drones to follow.

The above-mentioned cases [9], [11], [12], [13] and [14] propose a docking in a slow pace. As mentioned before, the capacity of docking with high velocity is also considered on this thesis.

Achieving stable contact at higher speeds than already existing research means a reduction of the inspection time; the drone would not be required to slow down before making contact with a wall as they can dock even tilted. Consequently, maximizing the life battery usage.

22

(24)

(25)

Chapter 3 Implementation

The simulation environment consists of three main modules: Drone hybrid dynamics, flight control system and sensors. These modules are illustrated in figure 3.0.1 together with the command block, where the user inputs the desired references for the quadcopter to follow, environment block, where parameters not related with the quadcopter such as gravity or air density are defined and visualization block, where graphs and a virtual reality (VR) model of the quadcopter [17] display the drone performance. The following sections explain each of these main modules in more detail.

Figure 3.0.1: Overall simulation.

3.1 Drone hybrid dynamics

The quadcopter is modelled in two different ways depending on the circumstances; Free- flight and docking dynamics. Free-flight dynamics is when the drone is flying around with no physical contact with the environment while docking dynamics is when the drone stays in contact with the side wall.

The free-flight dynamics correctness is verified by [18] where quadcopter together with arm are explained, [19] and [20] where a quadcopter model is derived in order to be controlled.

The results obtained from the free-flight dynamics are compared against these for verification and validation purposes. Similarly, the docking dynamics are based on references [13] where reaction forces against an object are considered, [12] and [10] where a hybrid modelling approach is followed to represent the dynamics. Their resulting data is verified and validated by comparing against them.

24

(26)

3.1.1 Free-flight dynamics

To obtain the equation of motions of the quadcopter, two frames are presented; inertial and body frames. The inertial frame, with components x, y and z, is fixed to the ground in which the gravity force goes along its positive z axis. The body frame, with components x_B, y_B, and z_B, is attached to the quadcopter where its origin coincides with the drone COM.

Figure 3.1.1: Inertial and body frames, thrusts and angular velocities.

The linear and angular positions with respect to the inertial frame are defined as ⃗x and ⃗η respectively . The quadcopter attitude is represented by Euler angles; ϕ, roll or rotation along x axis, θ denotes pitch or rotation along the y axis and ψ, yaw or rotation along z axis.

⃗ x =



x y z



 , ⃗η =



ϕ θ ψ



 (3.1)

Similarly, the linear position and angular velocity in the body frame are defined as ⃗x_B and

⃗ ω.

⃗ x_B =



xB

y_B z_B



 , ⃗ω =



p q r



 (3.2)

25

(27)

In order to represent vectors on the body frame as coordinates on the inertial frame and vice versa, the following rotation matrix is required:

⃗x = ⃗˙ R ˙⃗x_B, R =⃗



c_ψc_θ c_ψs_θs_ϕ− sψc_ϕ c_ψs_θc_ϕ+ s_ψs_ϕ s_ψc_θ s_ψs_θs_ϕ+ c_ψc_ϕ s_ψs_θc_ϕ− cψs_ϕ

−sθ c_θs_ϕ c_θc_ϕ



 (3.3)

Notice that c and s is the short representation of cosine and sine respectively. To represent angular velocities from one frame to the other, the transformation matrix ⃗W is a requisite:

⃗˙η = ⃗W ⃗ω, W =⃗





1 s_ϕt_θ c_ϕt_θ 0 c_ϕ −sϕ

0 ^s_c^ϕ

θ

cϕ

cθ



 (3.4)

Where tangent is represented by t. The relationship between the thrust generated by the rotor and its angular velocity squared is linear:

F_i = bω²_i (3.5)

Where i denotes the number of the propeller depicted in figure 3.1.1 and b is a constant dependant on the torque proportionality constant Ct, fluid density ρ, area swept out by the rotor As and radius of the propeller R:

b = C_tρA_sR² (3.6)

The resistance to advance throughout the air is also considered and represented as a drag force due to the linear motion friction:

F_drag_iB = 1

2ρA_iC_d_i˙i²_B (3.7)

Where i represents the direction on the body frame, A_i the frontal area and C_d_i the drag coefficient, dependant on the quadcopter aerodynamic shape. Therefore, the resultant drag force in the dimensional space goes as follows:

F⃗drag =





−Fdrag_xBsign( ˙x_B)

−Fdrag_yBsign( ˙y_B)

−Fdrag_zBsign( ˙z_B)



 (3.8)

The torque generated by a rotor respect the z_B axis is:

M_i = kω_i²+ I_Mω˙_i (3.9)

Where IM is the rotor inertia and k a drag coefficient dependant on a dimensionless constant Ct, fluid density ρ, cross-section area of propellers A_cross and propeller radius R:

k = C_tρA_crossR³ (3.10)

26

(28)

The effect due to ˙ω_iin equation 3.9 can be neglected as considered minimal. Therefore, the total torque along the zB axis simplifies as:

Mψ =

∑4 i=1

kω_i² (3.11)

To compensate the overall yaw movement, two rotors must rotate clockwise and the others counterclockwise (see appendix A). Consequently, if all four rotors spin at the same angular velocity, the quadcopter remains steady, without yawing.

The derivation of torque generated along the x_B and y_B follows the same principle. They depend on the thrust and distance of each rotor with respect to the specific axis:

M_ϕ= L_x(F₁− F2− F3+ F₄) (3.12) M_θ = L_y(F₁+ F₂− F3− F4) (3.13) Where L_xand L_y are the minimum distance of the rotors respect the x and y axis respectively.

The quadcopter inertia matrix is described as:

I⃗_drone =



 I_xx −Ixy −Ixz

−Ixy I_yy −Iyz

−Ixz −Iyz I_zz



 (3.14)

Due to the drone symmetry in x_B-z_B plane, the products of inertia I_xy = I_yz = 0. Symmetry respect y_B-z_Bplane cannot be guaranteed due to the further mechanical arm addition. However, Iyz = 0 is a valid approximation as Iyz effects are negligible.

Arm dynamics

The arm addition onto the UAV leads to some parameters modification. First of all the mass:

m_T = m_drone+ m_arm (3.15)

With m_droneand m_armmass of the drone and arm respectively. The inertia is also influenced by this attachment:

I⃗_arm = ⃗I_arm_COM + m_arm



d_xx² 0 0 0 dyy2

0 0 0 d_zz²



 (3.16)

I⃗_arm is derived by applying Huygens-Steiner theorem or parallel axis theorem where ⃗I_arm_COM is the inertia of the arm respect its COM and d_xx, d_yy and d_zz the distances between the axis respect the drone and arm COM. For simplification purposes the calculation is considered as if the arm remains parallel with respect to the x-y plane of the body frame. This is due the pitch angle being considered too small for the inertia difference to be noticeable. Therefore, the total inertia derives as:

I⃗_T = ⃗I_drone+ ⃗I_arm (3.17)

27

(29)

Furthermore, the arm addition not only generates a new force, ⃗F_arm, but also a torque, ⃗M_arm since the line action of this force is applied off the drone body frame origin.

M⃗_arm= ⃗r_arm× ( ⃗R⁻¹F⃗_arm) = ⃗r_arm× [

R⃗⁻¹ (

m_arm



0 0 g



 )]

(3.18)

Where g is the gravity acceleration and ⃗r_armthe distance from the arm COM to the body frame origin represented as:

⃗rarm =



 a + ^L^arm₂ cos(φ) 0

−c − ^L^arm₂ sin(φ)



 , (3.19)

Being a the distance from the connection point between arm and drone to drone COM on the x_B direction, L_arm the arm total length, φ the arm angular position with respect to the x_B axis and c the distance from the connection point between arm and drone to drone COM on the z_Bdirection. The ability of the drone arm to remain parallel to the ground plane or perpendicular to the wall independently of the quadcopter attitude is assumed. Therefore, φ can be considered equal to−θ, being φ positive in the direction shown in figure 3.1.2.

Figure 3.1.2: L_φ, M_react, ⃗F_arm and φ angle.

A reaction torque due to the arm movement denoted as M_react can be also contemplated.

However, the rotation joint allows us to disregard this torque since the arm movement ideally does not imply a reaction (rotates freely with no friction). Consequently, in terms of rotational motion, only the pitch is affected by the arm addition, since M_armis applied on the y axis.

28

(30)

3.1.2 Docking dynamics

Additional equations to the free-flight dynamics constitute the docking dynamics. This section determines the docking dynamics using the arm with a single support followed by the three-support arm.

Single-support arm

Supposing a single-support arm between the drone and the wall, as depicted in figure 1.5.1a, a reaction force ⃗Fwall arises. This force most likely generates a torque as its line of application is usually nonconcurrent with the drone COM. Only when θ =−0.359rad the line of the force application coincides with the COM. A spring-damper system is considered onto the mechanical arm tip. Therefore, F_wallon the inertial frame can be represented as:

F⃗_wall=



−(kspringx + d_damper˙x) 0

0



 (3.20)

With k_spring and d_damper are the translational stiffness and damping of the spring-damper system respectively. Since docking on a wall supposes an equilibrium, the ⃗F_wall eventually becomes:

F⃗wall = B11, where B = ⃗⃗ R



 0

0

F₁+ F₂+ F₃+ F₄



 (3.21)

That is to say, the force exerted by the drone in the perpendicular direction to the wall is equal to the emergent force reaction. The distance between the contact point where ⃗F_wallis generated and the drone COM is denoted as ⃗r_contact. Consequently, the torque generated is:

M⃗wall= ⃗rcontact× ⃗Fwall, ⃗rcontact =



L_wall_x + L_arm 0 Lwallz



 (3.22)

L_wall_x and L_wall_z determines the distance of the joint between the arm and the drone and the drone COM in the x and z direction with respect to the inertial frame. β is the angle originated from these two distances, it can be represented as:

β =−atan(c a

)

+ φ =−atan(c a

)− θ (3.23)

29

(31)

Figure 3.1.3: L_wall and F_wall.

Therefore, L_wall_x and L_wall_z can be defined by trigonometry as:

L_wall_x =√

a²+ c²cos(β) L_wall_z =√

a²+ c²sin(β) (3.24)

Three-support arm

For the case of the three-support arm, illustrated in figure 1.5.1b, the reaction force is divided among three contact points instead. This is represented as:

F⃗_wall₃ = ⃗F_wall_middle + 2 ⃗F_wall_side (3.25)

Where ⃗F_wall_middle and ⃗F_wall_side represents the contact force produced on the middle support and side support respectively. Similarly as the other arm design, a spring-damper system is mounted on the supports such as:

F⃗wall_middle =



−(kspring1x + d_damper₁˙x) 0

0





F⃗_wall_side =



−(kspring2x + ddamper2˙x) 0

0





(3.26)

With k_spring₁ and d_damper₁ the translational stiffness and damping of the middle support and k_spring₂ and d_damper₂ of the side contacts. The torque generated is a contribution of each reaction force:

30

(32)

M⃗_wall₃ = ⃗r_contact× ⃗Fwallmiddle + ⃗r_contact_right × ⃗Fwallside + ⃗r_contact_{lef t}× ⃗Fwallside (3.27) Where ⃗r_contact_right and ⃗r_contact_{lef t} correspond to the supports’position with respect to the quadcopter COM:

⃗r_contact_right =



L_wall_x + L_arm d_y L_wall_z − e



 , ⃗r_contact_{lef t} =



L_wall_x + L_arm

−dy

L_wall_z − e



 (3.28)

With d_y the distance between the side supports and the drone COM in the y direction and e the distance in z direction between the middle support and the side supports.

3.1.3 Equations of motion

The forces acting on the drone represented in the body frame are:

F⃗b = ⃗R⁻¹



 0 0 m_Tg



 −



 0

0

F₁+ F₂+ F₃+ F₄



 + ⃗F_drag + ⃗R⁻¹F⃗wall (3.29)

Similarly, the torques are:

T⃗_b =



M_ϕ M_θ M_ψ



 + ⃗M_arm+ ⃗R⁻¹M⃗_wall (3.30)

Considering the quadcopter as a rigid body, the Newton-Euler equations are applied to derive its dynamics. Regarding the linear motion:

m_T⃗x¨_B = ⃗F_b− ⃗ω × (mT⃗x˙_B) (3.31) Whereas the rotational motion is defined as:

⃗I_T⃗ω = ⃗˙ T_b− ⃗ω × (⃗IT⃗ω) (3.32) Where ⃗ω× (mdrone⃗x˙_B) and ˙⃗ω× (⃗IT⃗ω) terms represent the centripetal forces in both motions.

Notice that both equations of motion are derived with respect to the body frame and considering the single-support arm. To represent the three-support arm dynamics instead, ⃗Fwall and ⃗Mwall

must be substituted by ⃗F_wall₃ and ⃗M_wall₃ in equations 3.29 and 3.30.

31

(33)

3.2 Flight control system

The simulation is provided with the capacity of changing the control strategy depending on the situation, i.e. a hybrid model is implemented. Due to the change of dynamics between free- flight and docking, different control strategies are implemented for each. All the simulation is performed using a sampling time of T_s = 0.005s, capable for the Intel® aero compute board to manage [21]. The flight control system (FCS block in figure 3.0.1) receives the command from the user who defines the desired altitude and attitude. This goes in form of z position, roll, pitch and yaw orientation. The measured data from the sensors block (figure 3.0.1) is also inputted.

Inside FCS block, the sensor data is transformed into an estimation of the states in the estimator block (figure 3.2.1). The controller block then receives the references and the states estimation (together with the real states for hybrid model performance and comparison purposes).

Figure 3.2.1: Flight control system inside.

The controller contains the quadcopter hybrid model capability (figure 3.2.2). Either the free-flight or docking control is performed depending on the quadcopter x position. A virtual wall is defined at position x = 1.5m. Therefore, if the position is greater than 1.5m the docking control commands the quadcopter. Otherwise, the free-flight control governs the quadcopter motions. Notice that the actual position is subtracted by the variable init.posN ED(1, 1). This is because the drone does not start in coordinates (0, 0, 0). To actually know that the drone advance 1.5m this operation must be considered.

32

(34)

Figure 3.2.2: Controller block inside.

Getting into docking block (figure 3.2.3), the control strategy that currently performs is selected by variable value. I.e. a variable selects the strategy depending on its value; 0 for PID control, 1 for SMC and 2 for MPC.

Figure 3.2.3: Docking block inside.

33

(35)

From here, all control strategies follow the same idea. As seen in figure 3.2.4, there are four independent controllers; one for altitude or z and three for each angle of motion (roll, pitch and yaw). These can be found in blocks yaw, altitude and pitch/roll. The total thrust and torque in each motion required to achieve the desired reference constitute the control signals. Hence, these signal are in terms of N and N m. The commands from the user are the references of the controller.

Figure 3.2.4: Control idea.

Inside Control Mixer block, figure 3.2.5, a matrix multiplication is performed. The goal of the matrix multiplication is to dictate each rotors thrust contribution to approximate to the desired references. This matrix is built up from the following equations:





 T_total

M_ψ M_θ M_ϕ





= M





 F₁ F₂ F₃ F₄





,





 T_total

M_ψ M_θ M_ϕ





=







1 1 1 1

k/b −k/b k/b −k/b L_y L_y −Ly −Ly

L_x −Lx −Lx L_x











 F₁ F₂ F₃ F₄





 (3.33)

Where T_total represents the total thrust generated by the four rotors. In this case, to get the desired thrust per each motor the required torque and total thrust are multiplied by M⁻¹:





 F₁ F₂ F3

F₄





= M⁻¹





 T_total

M_ψ Mθ

M_ϕ





 (3.34)

34

(36)

Figure 3.2.5 illustrates how this M⁻¹ matrix is represented by the variable Controller.Q2T s performed in an m-file on Matlab interface.

Figure 3.2.5: Control mixer block inside.

The thrust obtained is further divided by the parameter b to obtain the angular velocity squared of each rotor and multiply by vector ⃗s =[

1 −1 1 −1]

, which remarks the spinning direction of the positive thrust of each rotor. Therefore, the angular velocity goes as an input of the quadcopter plant model where the real states are outputted and further fed back again into the system. Following, an explanation of how each control strategies is implemented and how their parameters are obtained.

3.2.1 Free-flight control

A PID control is developed in order to make the drone capable of flying and approach the wall. For this aim, a linearization is accomplished for the further PID values acquisition. This topic is not explained as it is out of the thesis purpose.

3.2.2 Docking control

For the docking control three different control strategies are considered; PID control, SMC and MPC. Their derivation is detailed hereafter. For the sake of simplicity the single-support arm case is only demonstrated.

35

(37)

PID control

The PID controller is without any doubt the most applied controller in engineering. The main advantage is its easy implementation; it is essentially some gain multiplied by the error of the targeted state to track. Nevertheless, they show a decent robustness performance. Possibly the main disadvantage is the loss of modelling precision due to the exclusion of nonlinearities throughout the linearization; as in the free-flight control, a linearization is accomplished.

Figure 3.2.6: Typical PID block diagram, not real implementation.

This linearization allows to get the most appropriated values for the PID control. However, an equilibrium point must be chosen, hence, causing controlling imprecision far from this equilibrium. The chosen states for the linearization are:

⃗ x =[

ϕ θ ψ ˙x_B y˙_B ˙z_B x y z p q r]T

(3.35) Whereas the inputs go as follows:

⃗u =[

w₁² −w2² w₃² −w4² ρ g]T

(3.36) Notice the negative sign in w2 and w4. This is due to the torque reaction of the rotors, which makes the drone yaw (see Appendix A). Hence, two rotate clockwise and two counterclockwise to compensate the reaction.

36

(38)

Expanding equations 3.3, 3.4, 3.31 and 3.32 and considering the different direction of rotation of the rotors the following set of equations are obtained:

f (⃗x, ⃗u) =











ϕ = p + s˙ _ϕt_θq + c_ϕt_θr θ = c˙ _ϕq− sϕr

ψ =˙ ^s^ϕ

cθq + ^c^ϕ

cθr

˙x_B=−sθg +_m¹

T

(−¹₂ρA_xC_d_x˙x²_Bsign( ˙x_B)− cψc_θ(k_springx + d_damper˙x)

)− q ˙z_B+ r ˙y_B

˙

y_B= s_ϕc_θg +_m¹

T

(−¹₂ρA_yC_d_yy˙²_Bsign( ˙y_B)− (cψs_θs_ϕ− sψc_ϕ)(k_springx + d_damper˙x) )

−r ˙x_B+ p ˙z_B

˙z_B= c_ϕc_θg +_m¹

T

(− CtρA_sR²(w₁²+ w₂²+ w₃²+ w₄²)−¹₂ρA_zC_d_z˙z_B²sign( ˙z_B)

−(cψs_θc_ϕ+ s_ψs_ϕ)(k_springx + d_damper˙x)

)− p ˙y_B+ q ˙x_B

˙x = c_ψc_θ˙x_B+ (c_ψs_θs_ϕ− sψc_ϕ) ˙y_B+ (c_ψs_θc_ϕ+ s_ψs_ϕ) ˙z_B

˙

y = s_ψc_θ˙x_B+ (s_ψs_θs_ϕ+ c_ψc_ϕ) ˙y_B+ (s_ψs_θc_ϕ− cψs_ϕ) ˙z_B

˙z =−sθ˙x_B+ c_θs_ϕy˙_B+ c_θc_ϕ˙z_B

˙ p = _I¹

xxT

(

L_xC_tρA_sR²(w²₁− w²2− w²3+ w₄²)− (−c +^L^arm₂ s_θ)c_θs_ϕm_armg

−sψc_θ√

a²+ c²s_β(k_springx + d_damper˙x)− qr(IzzT − IyyT) )

˙ q = _I¹

yyT

(

L_yC_tρA_sR²(w₁²+ w²₂− w²3− w²4)− (−c +^L^arm₂ s_θ)s_θm_armg

−(a +^L^arm₂ c_θ)c_θc_ϕm_armg− (sψs_θs_ϕ+ c_ψc_ϕ)√

a²+ c²s_β(k_springx + d_damper˙x)

−pr(IxxT − IzzT) )

˙r = _I¹

zzT

(

C_tρA_crossR³(−w1²+ w²₂− w²3+ w₄²) + (a +^L^arm₂ c_θ)c_θs_ϕm_armg

−(sψs_θc_ϕ− cψs_ϕ)√

a²+ c²s_β(k_springx + d_damper˙x)− pq(IyyT − IxxT) )

(3.37) In these equations, w₁, w₃positive and w₂, w₄negative are considered to contribute to the lifting thrust positively. Bear also in mind that the rotation matrix ⃗R is orthogonal, thus, ⃗R^T = ⃗R⁻¹. Assuming the case for small angles, the equations can be simplified by applying the following approximations:

sin(i) ≈ i, cos(i) ≈ 1, i² ≈ 0 (3.38) Where i represents a small Euler angle. These approximations help to simplify the representative equations of the quadcopter dynamics by reducing the nonlinearities that sines and cosines bring with them. Making this way the equations affected by these trigonometry functions depending linearly of the states instead.

37

Control strategies evaluation for stabilizing a quadcopter docking on a side wall.

Control strategies evaluation for stabilizing a quadcopter docking on a side wall

EDUARDO FUENTEVILLA BLANCO

Sammanfattning

Abstract

Acknowledgements

Abbreviations

Definitions

Contents

List of Figures

List of Tables

Chapter 1 Introduction

1.1 Background

1.2 Purpose

1.3 Research question

1.4 Delimitations

1.5 Method

Chapter 2

Frame of reference

Chapter 3

Implementation

3.1 Drone hybrid dynamics

3.1.1 Free-flight dynamics

3.1.2 Docking dynamics

3.1.3 Equations of motion

3.2 Flight control system

3.2.1 Free-flight control

3.2.2 Docking control