BélaLantos Ph.D.Dissertation ZsófiaBodó ModernControlMethodsforUnmannedAerialandGroundVehicles

Department of Telecommunications and Media Informatics

Modern Control Methods for Unmanned Aerial and Ground Vehicles

Zsófia Bodó

Ph.D. Dissertation

Supervised by

Béla Lantos

Professor Emeritus

Budapest, Hungary 2021

Declaration

I hereby certify that this material, which I now submit for assessment on the programme of study leading to the award of PhD is entirely my own work and has not been taken from the work of others save and to the extent that such work has been cited and acknowledged within the text of my work.

Nyilatkozat

Alulírott Bodó Zsófia kijelentem, hogy ezt a doktori értekezést magam készítettem, as abban csak a megadott forrásokat használtam fel. Minden olyan részt, amelyet szószerint, vagy azonos tartalomban, de átfogalmazva más forrásból átvettem, egyértelm˝uen, a forrás megadásával megjelöltem.

Budapest, 22. 06. 2021

...

Zsófia Bodó

i

I Abstract

The control of autonomous unmanned ground (UGV) and air (UAV) vehicles are part of the non-linear dynamic control methods. It integrates the knowledge of various fields, and its research is in the center of the international trends. Its main topic is the control of various vehicles (mobile robot agents) based on common grounds including modeling, identification, path design considering the under-actuated fashion of the agents and the obstacles, navigation based on special sensors, movement analysis and sensor-fusion, fast communication for control and system optimization, as well as the coordinated control of several vehicles in formation considering static and moving obstacles.

The main goal of this research is to develop control methods based on common system engineering grounds for ground and air vehicles. Movement in formation, optimal strategy selection, remote connection, control and navigation should be also considered to be part of the research.

Based on experience it can be seen that autonomous embedded control systems can be designed for autonomous mobile robot agents, obstacle avoidance of vehicles in traffic, quadrotor copters and propelled aeroplanes and their movement in formation with the use of modern control method development tools, embedded microprocessors and GPS/IMU sensors.

ii

II Kivonat

Az önálló m˝uködésre képes embernélküli földi (UGV) és légi (UAV) járm˝uvek irányítása a nemlineáris dinamikus rendszerek irányításelméletének körébe tartozik, integrálja számos más terület tudását, kutatása a nemzetközi trendek középpontjában áll. Központi témája a különféle járm˝uvek (mobilis robot ágensek) egységes elvek alapján történ˝o modellezése, identifikációja és irányítása, az ágensek alulaktuált jellegét és az akadályokat is figyelembe vev˝o pályatervezés, a speciális érzékel˝okre, mozgásanalízisre és szenzorfúzióra alapuló navigáció, a gyors irányítási célú kommunikáció és a rendszeroptimalizálás, valamint több járm˝u formációban haladásának koordinált irányítása statikus és mozgó akadályok esetén.

A kutatás f˝o célja a földi és légi járm˝uvek esetére egységes rendszertechnikai elvek alapján tervezhet˝o irányítási módszerek kifejlesztése. Megvizsgálandók a formációban haladás, a koalíciók közötti optimális stratégiaválasztás, a távolról történ˝o kapcsolattartás és navigáció lehet˝oségei.

A tapasztalatok alapján autonóm beágyazott irányítási rendszerek fejleszthet˝ok ki auto- nóm mobilis robot ágnesek irányítására, közúti járm˝uvek akadályelkerülésére, 4-rotoros helikopterek és motoros repül˝ogépek irányítására és formációban haladására, korszer˝u irányítástechnikai fejleszt˝oeszközök, beágyazott processzorok és GPS/IMU szenzorok felhasználásával.

iii

III Acknowledgements

During the years of research work I faced quite some professional challenges. However I am grateful to say that I had many people around me whom I could lean on during the hardships.

First of all I would like to express my gratitude to my supervisor, prof. Bela Lantos for the professional help during our work together. I would like to thank him that amazing control theory knowledge that I received throughout the years.

I would like to thank to dr. Balint Kiss and the Department of Control Engineering the years I could spend there and the countless opportunities for professional growth.

I would like to thank my family and my parents that they expressed their support during mt university years. Thank you that you are there for me as a professional role model with your company and university work.

I would also like to thank the community of PhD students at the university. I felt really supported, from some LaTex support to some professional coffee breaks.

iv

IV Köszönetnyilvánítás

A kutatómunka során számos szakmai kihívással szembesültem, azonban sok ember volt körülöttem, akikre támszkodhattam a nehézségek során.

Mindenek el˝ott szeretném megköszönni témavezet˝omnek, dr. Lantos Bélának az éveken át tartó szakmai támogatást. Köszönöm mindazt az elképeszt˝o mennyiség˝u irányítástechni- kai tudást, amit a közös munka során kaphattam.

Köszönöm dr. Kiss Bálint vezetésével az Irányítástechnika Tanszéknek az ott tölött éveket és a rengeteg szakmai fejl˝odési lehet˝oséget.

Köszönöm családomnak, szüleimnek, hogy az egyetemi éveim alatt kitartóan segítet- tek céljaimban. Köszönöm, hogy kiskorom óta példaképként állnak el˝ottem szakmai és egyetemi munkájukkal.

Köszönöm az egyetemi doktorandusz közösségnek az elmúlt évek alatt nyújtott segít- séget - legyen szó akár egy LaTex formázási kérdésr˝ol, akár egy szakmai kávézásról.

v

Contents

1 Introduction 1

1.1 Major research directions, results . . . 2

1.2 Organization . . . 3

1.3 System Engineering Fundamentals of Vehicles . . . 3

1.3.1 Dynamic Model Based on Newton–Euler Equations . . . 4

1.4 Orientation Description by Rotations and Quaternion . . . 6

1.4.1 Homogeneous Transformations . . . 7

1.4.2 Orientation Description Using Elementary Rotations . . . 8

1.4.3 Rodrigues formula . . . 8

1.4.4 Kinematic Model Using Euler (RPY) Angles . . . 9

1.4.5 Kinematic Model Using Quaternion . . . 10

1.4.6 Conversion Between Orientation Descriptions . . . 11

2 Control of Quadrotor UAVs 13 2.1 Dynamic model of quadrotors . . . 13

2.1.1 Kinematic model of aerial vehicles . . . 13

2.1.2 Dynamic model . . . 14

2.1.3 Simplified dynamic model . . . 15

2.1.4 Rotor dynamics . . . 16

2.2 Limited state input/output linearization . . . 16

2.3 Standard integral backstepping . . . 16

2.4 Full state backstepping control . . . 18

2.4.1 Applying backstepping algorithm to the helicopter . . . 18

2.4.2 Position control . . . 20

2.4.3 Attitude control . . . 22

2.4.4 A simplified control of the position and attitude . . . 22

2.4.5 Rotor control . . . 23

2.4.6 Tuning of controller parameters . . . 23

2.4.7 State estimation . . . 23

2.5 Path design and tracking . . . 24

2.5.1 Path design with continuous acceleration . . . 24

2.5.2 Path design with continuous jerk . . . 25

2.5.3 The tracking algorithm with filtering and multiple differentiation 26 2.6 Adaptive control . . . 26

2.6.1 Modeling the parameter changes . . . 27

2.6.2 Mass and vertical force identification . . . 27

2.7 Simulation tests . . . 29 vi

2.7.2 Estimation of the initial helicopter mass . . . 29

2.7.3 Estimation of varying mass and vertical disturbance force . . . . 30

2.7.4 Attitude and Rotor Control . . . 30

2.8 Embedded Control Realization . . . 32

2.8.1 Control architecture . . . 32

2.8.2 Hardware-in-the-loop test . . . 33

2.8.3 Reachable sampling time for the control algorithm . . . 33

2.9 Summary . . . 34

3 Fixed wing UAV control 38 3.1 Dynamic Model of Airplanes . . . 39

3.1.1 Navigation Fundamentals . . . 39

3.1.2 Wind-Axes Coordinate System . . . 39

3.1.3 Wind Effect Modeling . . . 41

3.1.4 Aerodynamic Forces and Torques . . . 41

3.1.5 Gyroscopic Effect of Rotary Engine . . . 43

3.1.6 Flat Earth State Equations of Airplane . . . 44

3.2 Parametrization of Aerodynamic and Thrust Forces and Moments . . . . 46

3.2.1 Parametrization of the Beaver Aircraft . . . 46

3.2.2 Parametrization of Small UAVs . . . 46

3.2.3 Parametrization of Sekwa UAV . . . 47

3.3 Attitude Control of Fixed Wing UAVs . . . 49

3.3.1 Euler angles . . . 50

3.3.2 Quaternion exponential and logarithm . . . 50

3.3.3 Standard Integral Backstepping Control using Euler angles . . . . 51

3.3.4 Attitude control without the use of quaternion logarithm . . . 53

3.3.5 Attitude control using quaternion logarithm . . . 55

3.4 Altitude control of fixed wing UAVs . . . 56

3.4.1 Control concept . . . 56

3.4.2 Computation details . . . 57

3.5 Reference Trajectory Design . . . 57

3.5.1 Straight line motion . . . 58

3.5.2 Circle motion . . . 58

3.5.3 Spiral motion . . . 58

3.5.4 Linear and angular velocity to the same axis . . . 59

3.5.5 Smooth Connection of Elemental Sections and Centrifugal Force Compensation . . . 59

3.6 Simulation experiment . . . 60

3.6.1 Control experiment without the use of log(q) . . . 60

3.6.2 Control experiment with the use of log(q) . . . 62

3.7 Summary . . . 66

4 Ground vehicle control 73 4.1 Modified Kinematic Control . . . 74

4.1.1 Modified error definition for the chain form . . . 75

4.1.2 Kinematic control in the presence of sliding effects . . . 75 vii

4.1.3 Robust kinematic control . . . 76

4.2 Applied Dynamic Control . . . 77

4.2.1 Input affine dynamic model for control design . . . 78

4.2.2 Nominal dynamic control saving kinematic steering angle . . . . 78

4.2.3 Differential Geometry Based Control Algorithm . . . 79

4.2.4 Flatness control . . . 80

4.3 Numerical Results . . . 85

4.3.1 Checking the available data . . . 85

4.3.2 The nonlinear dynamic model and the DGA control . . . 86

4.3.3 Simulation results . . . 87

4.4 Summary . . . 91

5 Nonlinear State Estimation with Sensor Fusion 95 5.1 INS Navigation . . . 96

5.1.1 Differential equations of the orientation descriptions . . . 96

5.1.2 Long distance INS navigation . . . 96

5.1.3 Nonlinear state equations of long distance INS navigation . . . . 97

5.1.4 Integrating magnetic sensor measurements . . . 98

5.2 State Estimation Using EKFs . . . 99

5.2.1 The EKF algorithm . . . 99

5.2.2 Observation model . . . 100

5.3 Implementation Concept of INS Navigation . . . 101

5.4 Experimental Results Using UAV Flight Data . . . 103

5.5 Nonlinear state equations of Flat Earth navigation . . . 109

5.6 Summary . . . 110

6 Conclusions 112 Bibliography 116 Publications 117 Appendix A Derivation of the Generalized Kinematic Control i A.1 The original chain form of car-like robots . . . i

A.2 Kinematic modeling and control in case of slipping . . . iii

Appendix B Stability Proof for DGA Control of Cars v B.1 Input Affine Model for Small Angles . . . v

B.2 Differential Geometry Based Control Algorithm . . . vi

B.2.1 External State Feedback Design . . . vii

B.2.2 Stability Proof of Zero Dynamics . . . viii

viii

Chapter 1 Introduction

Research and development of unmanned vehicles is highly prospering these days. Control of such vehicles integrates the knowledge of various fields, and its research is at the center of international trends. Beside military applications, civilian solutions are gaining popularity rapidly. Although most of the available consumer devices and industrial equipment has some autonomous features, the growing demand on fully autonomous vehicles both in air or land increased the need for more reliable, more robust control algorithms. The main research topic is the control of various vehicles (mobile robot agents) based on common grounds including modeling, identification, path design considering the under- actuated fashion of the agents and the obstacles on the path, navigation based on special sensors, movement analysis and sensor-fusion, fast communication for control and system optimization, as well as the coordinated control considering static and moving obstacles.

To provide full autonomy to the vehicle, on-board processing would be ideal so the vehicle can operate without the direct contact for a base station. Although on-board com- puters have reduced computation availability, recent developments of the silicon industry are turning this trend to a new direction. Today’s microcomputer and microcontroller solutions are powerful enough to support control methods with complicated calculations.

Currently available sensors are smaller and cheaper than their predecessors thus giving the possibility of gathering detailed information on the environment on-board the vehicle.

This supports the latest calculation methods to determine the current and predict the future state of the unmanned vehicle.

This work discusses the latest hierarchical control methods and introduces refined calculations and algorithms in both attitude and position control for quadrotor UAVs, fixed wing UAVs and general UGVs.

The control of autonomous unmanned ground (UGV) and air (UAV) vehicles is part of the non-linear dynamic control methods.

In the domain of nonlinear control algorithms, the most popular technique is the backstepping approach, although several other methods are elaborated including sliding mode, described in the conference paper of Boubadallah and Siegwart [1] and feedback linearization control algorithms introduced by Das, Subbarao and Lewis in their work [2]. This thesis however presents refined Backstepping Control (BSC) and the differences between Euler Angle and Quaternion based calculations. In the domain of quadrotor UAVs a full state backstepping algorithm is discussed which was introduced in the paper of Madani and Benallegue [3]. While the modeling and control of fixed wing UAVs builds on the literature of Cook [4] or Setevens and Lewis [5], Control Lyapunov Function based

2 1.2 Major research directions, results BSC and non-integrating BSC methods presented building on the PhD Thesis of Harkegard [6] and Peddle [7]. Dynamic models of fixed wing aircraft that are used for simulations are found in the MSc Thesis of Blaauw [8].

Popular control methods of unmanned ground vehicles are using PID-type control of line keeping [9], potential field technique [10] and nonlinear time-optimal control [11].

This thesis discusses a modified kinematic control method with an approach to estimate the errors caused by the sliding effect. While in most cases this sliding effect is not considered in kinematic models, like in the work of De Luca and coworkers [12], the method of Arogeti and Berman [13] is a remarkable exception. Their work is based on the research of Scherer and Weiland [14] in the field of peak-to-peakL_∞(or generalizedH₂) disturbance effects in single variable (SISO) systems.

Since state estimation is an important step in the used modeling and control methods, a complete chapter is dedicated for nonlinear state estimation that is based on sensor fusion. Popular approaches in state estimation are based on EKF (Extended Kalman Filter) described in the book Theory and Design of Control Systems II. by Lantos [15], sigma- point estimators like UKF (Unscented Kalman Filter) introduced in the paper of van der Merwe, Wan and Julier [16] and simmetry-preserving observers (SPO) using Lie-Group technique shown in the article by Bonnabel and coworkers [17]. In this dissertation an improved EKF method is introduced for UAVs based on the WGS-84 standard of GPS navigation.

Before any of these control methods could be discussed, the dynamic models of the vehicles are introduced respectively. Simplifications, where needed, are pointed out in respect of the chosen model or control method. The introduced models are not only needed to introduce the control methods, but they are also necessary for building an adequate simulation environment to support development of real life applications without the risk of harming people or equipment. Feeding collected telemetry data from real life applications, the simulation can help refine both the models and control methods. This technique also can be used to prove the working of the control algorithms.

1.1 Major research directions, results

The main goal of this research is to develop control methods based on common system engineering grounds for ground and air vehicles.

Based on experience it can be seen that autonomous embedded control systems can be designed for autonomous mobile robot agents, obstacle avoidance of vehicles in traf- fic, quadrotor copters and propelled aeroplanes with the use of modern control method development tools, embedded microprocessors and GPS/IMU sensors.

Throughout the work the most important academic results of the control industry were studied and the results were used for this research, which was based on the control of three different types of unmanned vehicles - Quadrotor UAVs, Fixed wing UAVs and Unmanned Ground Vehicles. My main research was based on navigation and state estimation of vehicles, sensor fusion, hierarchical (on the upper level kinematic and on the lower lever dynamic model based) control of ground vehicles and input-output linearization based control of quadrotor helicopters.

1.2 Organization

This work consists of six main chapters. While this chapter introduce the System Engineer- ing Fundamentals of Vehicles, the following chapters 2, 3 and 4 present the research on the control of three different type of unmanned vehicles. They will discuss the different methods of low level (attitude) control to route/mission planning methods of quadrotor UAVs, fixed wing UAVs and finally unmanned ground vehicles. Chapter 5 introduces the nonlinear state estimation discussed in the highlight of UAV control. At the end of the dissertation the Bibliography and a list of my scientific publications can be found.

1.3 System Engineering Fundamentals of Vehicles

Vehicles and robots are designed for transportation and manipulation tasks respectively.

They can change the position or orientation of bodies in space. The transportation task can be formulated as the motions to some significant distance in comparison with the size of the moved bodies. Manipulation is to make any other change of the position and orientation of the bodies.

Vehicles are engineering systems used for transportation. They can be categorized in function of the environment where they perform the transportation, e.g. on the ground, in the water, in the air. A car or ground vehicle is a wheeled motor vehicle used for transporting passengers or cargo, which also carries its own engine or drive motor. An aircraft is a vehicle which is able to fly by being supported by the air. The most important type of aircraft are the airplanes and helicopters, in which the lift and thrust are supplied by control surfaces and one or more engine driven rotors.

The manipulation of an object can be solved using robots. A robot is an automatically guided machine, able to do tasks on its own. A robot is a re-programmable multi-functional manipulator designed to move materials, parts, tools, or specialized devices through variable programmed motions for the performance of a variety of tasks.

The bridge between the vehicles and robots is represented by the mobile robots which are robots that have the capability to move around in their environment and are not fixed to one physical location. Similarly to vehicles, they are categorized in the function of the environment in which they travel. Robots are usually called autonomous or unmanned if they have no driver (pilot). Autonomous engineering systems can perform desired tasks in partially known environments without continuous human supervision.

Nowadays almost all automatic control problems are solved using digital computers or micro-controllers. Most of vehicle and robot control algorithms require high computational costs and the sampling periods in order of milliseconds or even less. Hence such fast microprocessors should be applied that support floating point operations at hardware level.

Many of these types of processors are available these days at relatively low cost. The industrial microcontrollers beside the capability of fast computation also offers on-chip interfaces for sensors and actuators that can be used in automation.

There are a wide range of different type of sensors that facilitate the localization of vehicles in spaces such as the satellite based Global Positioning System based sensors, inertial sensors, laser or ultrasound based distance sensors, stereo vision systems. For precise localization of vehicles the signals from different (GPS, IMU, magnetometer, image etc.) sensors can be combined using sensor fusion techniques. The inertial system

4 1.3 System Engineering Fundamentals of Vehicles provides short term data, while the GPS based sensor corrects accumulated errors of the inertial system.

Common for many vehicles that the degree of freedom is six (3D position and 3D orientation) while the number of available actuators is smaller hence the vehicles are underactuated. For ground vehicles the actuators are the brake, acceleration and steering.

For ships (typically) the longitudinal thruster and the steering are the actuators. For a fixed wing aircraft the elevator, rudder and ailerons together with a (propeller) thruster while for a quadrotor helicopter the four (brushless DC) motors play the role of the actuators.

All in these cases the system is a nonlinear dynamic system that integrate the properties of the rigid body and extensions depending on the specialty of the limited actuators.

The control of vehicles and robots is a great challenge and it represents an important field of control engineering research in the past decades. The possibility to increase the autonomy of vehicles and robotic systems are facilitated by the new results and tools offered by the related technologies.

At this introductory level we summarize here some often used system engineering results regarding the dynamic model of rigid bodies and the characterization of the ori- entation for kinematic models based on the DCM (Direct Cosine Matrix), the Rodrigues formula, the navigational Euler angles and the quaternions. The proof of these results can be found in many standard works, see for example [18] and [15]. More special results will be discussed in later chapters for nonlinear control of autonomous vehicles.

1.3.1 Dynamic Model Based on Newton–Euler Equations

Consider a rigid body moving relative to an inertial coordinate systemK_I. To the rigid body an own coordinate systemK_Bis fixed in the center of gravity (COG) which is moving together with the rigid body. The velocity, acceleration, angular velocity and angular acceleration are defined relative to the inertial frame. The differentiation rule in moving coordinate system gives the relation between the formal (componentwise) derivatives in the moving frame and the derivatives in the inertial frame. Especiallya=v˙+ω×vwhere a,v,ω are the acceleration of COG, the velocity of COG and the angular velocity of the rigid body, respectively. The total external forceF_total and the total external torqueM_total are acting in COG.

All vectors are expressed in the basis of the moving frameK_B which will be shown by the simplified notation a,v,ω,F_total,M_total ∈K_B. Their components in K_B are a= (a_x,a_y,a_z)^T,v= (U,V,W)^T,ω = (P,Q,R)^T,F_total = (F_X,F_Y,F_Z)^T,M_total = (L,¯ M,N)^T. The rigid body has massmand inertia matrixI_cbelonging to COG.

The dynamic model of the rigid body is given by Newton’s force equation ma= m(v˙+ω×v) =F_total, and Euler’s moment equationI_cω˙ +ω×(I_cω) =M_total, from which follows

˙

v = −ω×v+ 1

mF_total, (1.1)

˙

ω = −I_c⁻¹ω×(I_cω) +I_c⁻¹M_total. (1.2)

Let us compute first the inverse of the inertia matrix:

I_c=





I_x −I_xy −I_xz

−I_xy I_y −I_yz

−I_xz −I_yz I_z



⇒ I_c⁻¹=





I₁ I₂ I₃ I₂ I₄ I₅ I₃ I₅ I₆



. (1.3)

det(I_c) = I_xI_yI_z−2I_xyI_xzI_yz−I_xI_yz² −I_yI_xz² −I_zI_xy², I₁ = (I_yI_z−I_yz²)/det(I_c),

I₂ = (I_xyI_z+I_xzI_yz)/det(I_c),

I₃ = (I_xyI_yz+I_xzI_y)/det(I_c), (1.4) I₄ = (I_xI_z−I_xz²)/det(I_c),

I₅ = (I_xI_yz+I_xyI_xz)/det(I_c), I₆ = (I_xI_y−I_xy²)/det(I_c).

Then we can continue with the computation of−ω×(I_cω):

−ω×(I_cω) =





0 R −Q

−R 0 P

Q −P 0











 I_x

−I_xy

−I_xz



P+





−I_xy I_y

−I_yz



Q+





−I_xz

−I_yz I_z



R





=





(−RI_xy+QI_xz)P+ (RI_y+QI_yz)Q+ (−RI_yz−QI_z)R (−RI_x−PI_xz)P+ (RI_xy−PI_yz)Q+ (RI_xz+PI_z)R (QI_x+PI_xy)P+ (−QI_xy−PI_y)Q+ (−QI_xz+PI_yz)R





=





(0)PP+I_xzPQ−I_xyPR+I_yzQQ+ (I_y−I_z)QR−I_yzRR

−I_xzPP−I_yzPQ+ (I_z−I_x)PR+ (0)QQ+I_xyQR+I_xzRR I_xyPP+ (I_x−I_y)PQ+I_yzPR−I_xyQQ−I_xzQR+ (0)RR



.

Dynamic model of the rigid with equations

U˙ = RV−QW+F_X/m,

V˙ = −RU+PW+F_Y/m, (1.5)

W˙ = QU−PV+F_Z/m,

P˙ = P_ppPP+P_pqPQ+P_prPR+P_qqQQ+P_qrQR+P_rrRR+I₁L¯+I₂M+I₃N, Q˙ = Q_ppPP+Q_pqPQ+Q_prPR+Q_qqQQ+Q_qrQR+Q_rrRR+I₂L¯+I₄M+I₅N,

R˙ = R_ppPP+R_pqPQ+R_prPR+R_qqQQ+R_qrQR+R_rrRR+I₃L¯+I₅M+I₆N, (1.6) where the parametersP_∗∗,Q_∗∗,R_∗∗are weighting factors for the products of the components

6 1.4 Orientation Description by Rotations and Quaternion ofω defined by

P_pp = −I_xzI₂+I_xyI₃,

P_pq = I_xzI₁−I_yzI₂+ (I_x−I_y)I₃, P_pr = −I_xyI₁+ (I_z−I_x)I₂+I_yzI₃,

P_qq = I_yzI₁−I_xyI₃, (1.7)

P_qr = (I_y−I_z)I₁+I_xyI₂−I_xzI₃, P_rr = −I_yzI₁+I_xzI₂,

Q_pp = −I_xzI₄+I_xyI₅,

Q_pq = I_xzI₂−I_yzI₄+ (I_x−I_y)I₅, Q_pr = −I_xyI₂+ (I_z−I_x)I₄+I_yzI₅,

Q_qq = I_yzI₂−I_xyI₅, (1.8)

Q_qr = (I_y−I_z)I₂+I_xyI₄−I_xzI₅, Q_rr = −I_yzI₂+I_xzI₄,

R_pp = −I_xzI₅+I_xyI₆,

R_pq = I_xzI₃−I_yzI₅+ (I_x−I_y)I₆, R_pr = −I_xyI₃+ (I_z−I_x)I₅+I_yzI₆,

R_qq = I_yzI₃−I_xyI₆, (1.9)

R_qr = (I_y−I_z)I₃+I_xyI₅−I_xzI₆, R_rr = −I_yzI₃+I_xzI₅.

For many aircraft, because of symmetry in the form, can be assumed thatK_xy=K_yz=0 and a lot of terms are zero inI_c⁻¹and inP_∗∗,Q_∗∗,R_∗∗ so that simplifications in the dynamic model are possible. However this is no more valid if the load is asymmetrical or varying.

The above equations do not consider the position and orientation of the rigid body, although many problems need also them, for example if a designed path has to be realized by the control. It means that r_c∈K_I and the orientation of K_B relative to K_I are also important. Hence we have to deal also with the kinematic part of the motion equation. For the description of the orientation the Euler (roll, pitch, yaw) angles and the quaternions are especially important.

1.4 Orientation Description by Rotations and Quaternion

According to [18] and [15], in the navigation of robots and vehicles the common practice is to use homogeneous transformations for describing the pose (position and orientation) of moving objects and rotations or quaternions for characterizing their orientation.

The kinematic differential equations use the frames of ECI (Earth Centered Inertia), ECEF (Earth Centered Earth Fixed), NED (North, East, Down) and Body (vehicle body) coordinate systems. In the sequel they will be referred by the letters i, e, n and b, respec- tively.

1.4.1 Homogeneous Transformations

Let us consider first two orthonormed coordinate systems (frames) denoted byKandK⁰ and let their basis vectors bei,j,kandi⁰,j⁰,k⁰, respectively.

Sincer=r⁰+pandr,p∈Kandr⁰∈K⁰therefore

r=ρ_xi+ρ_yj+ρ_zk, p= p_xi+p_yj+p_zk, r⁰=ρ_x⁰i⁰+ρ_y⁰j⁰+ρ_z⁰k⁰,

i⁰=r₁₁i+r₂₁j+r₃₁k, j⁰=r₁₂i+r₂₂j+r₃₂k, k⁰=r₁₃i+r₂₃j+r₃₃k, (1.10a) r = (r₁₁ρ_x⁰+r₁₂ρ_y⁰+r₁₃ρ_z⁰+p_x)i+

+(r₂₁ρ_x⁰+r₂₂ρ_y⁰+r₂₃ρ_z⁰+p_y)j+

+(r₃₁ρ_x⁰+r₃₂ρ_y⁰+r₃₃ρ_z⁰+p_z)k.

(1.10b)

These relations can also be described in matrix form:



 ρ_x ρ_y ρ_z



=





r₁₁ r₁₂ r₁₃ r₂₁ r₂₂ r₂₃ r₃₁ r₃₂ r₃₃







 ρ_x⁰ ρ_y⁰ ρ_z⁰



+



 p_x p_y p_z



,

↑i⁰ ↑ j⁰ ↑k⁰

| {z } described in i, j, kbasis

(1.11)

r=Rr⁰+p. (1.12)

Adding to them the identity 1=1 then the change of the orientation R betweenK andK⁰and the position vector pbetween the origins can be reduced to a homogeneous transformationT:







 ρ_x ρy

ρ_z 1









=









r₁₁ r₁₂ r₁₃ p_x r₂₁ r₂₂ r₂₃ p_y r₃₁ r₃₂ r₃₃ p_z

0 0 0 1















 ρ_x⁰ ρ_y⁰ ρ_z⁰ 1









, (1.13)

r 1

=

R p 0^T 1

r⁰ 1

=:

l m n p

0 0 0 1

r⁰ 1

=:T r⁰

1

, (1.14) r

1

=T r⁰

1

. (1.15)

Here(r^T 1)^T ∈R⁴and(r^0T 1)^T ∈R⁴denote the homogeneous coordinates ofr∈R³and r⁰∈R³.

Notice that for examplei⁰=r₁₁i+r₂₁j+r₃₁k⇒<i⁰,i>=r₁₁is the cosine of the angle between the unit vectors ofi⁰andietc. hence Rcan be rightly called the Direct Cosine Matrix (DCM).

The homogeneous transformationT has two important properties:

i) T gives the orientation and position ofK⁰relative toK.

ii) T makes it possible to determine the coordinates inKof the pointPif we know its coordinates inK⁰.

8 1.4 Orientation Description by Rotations and Quaternion It is useful to provide the homogeneous transformation T with indices in order to emphasize that the components ofl,m,n,pbelongingK⁰should be given in the basis ofK:

T_K,K0. It is apparent that the order of the indices is important in the interpretation.

The sequence of homogeneous transformations can be described by the product of homogeneous transformations:

T₁T₂=

R₁ p₁ 0^T 1

R₂ p₂ 0^T 1

=

R₁R₂ R₁p₂+p₁

0^T 1

. (1.16)

1.4.2 Orientation Description Using Elementary Rotations

Let us consider first the change oforientation. Let us assume that K and K⁰ originally coincided butK⁰has been rotated around some axis ofK by the angleϕ and consider the state after the rotation. In order to simplify the relations the following convention will be introduced hereafter:

C_ϕ :=cosϕ, S_ϕ :=sinϕ,C_{α β} :=cos(α+β),C₁₂:=cos(q₁+q₂)etc. (1.17) Using (1.11) the elementary rotations are as follows:

Rot(z,ϕ) = i⁰_rot j⁰_rot k_rot⁰

=





C_ϕ −S_ϕ 0 S_ϕ C_ϕ 0

0 0 1



, (1.18)

Rot(y,ϕ) = i⁰_rot j⁰_rot k_rot⁰

=





C_ϕ 0 S_ϕ

0 1 0

−S_ϕ 0 C_ϕ



, (1.19)

Rot(x,ϕ) = i⁰_rot j_rot⁰ k⁰_rot

=





1 0 0

0 C_ϕ −S_ϕ 0 S_ϕ C_ϕ



. (1.20)

1.4.3 Rodrigues formula

Let t be a general direction vector, ktk= 1. Let us assume that K and K⁰ originally coincided, butK⁰has been rotated aroundt by the angleϕ.

Letxbe the point to be rotated having projectionsx_t =t <t,x>andx_t⊥=x−x_t= x−t<t,x>. Here<t,x>=t^Tx=x^Ttdenotes the scalar product. Let the rotated image of x_t⊥in the plane orthogonal tot be denoted byx^rot_t⊥. Sincex_tdoes not change during rotation, hencex^rot =x_t+x^rot_t⊥. On the other handx_t⊥ andt×x_t⊥are two orthogonal directions in the plane orthogonal tot, furthermore kt×x_t⊥k=ktk · kx_t⊥k ·sin(90^◦) =kx_t⊥k, hence x_t⊥^rot =x_t⊥cosϕ+t×x_t⊥sinϕ, and as a consequence x^rot =t <t,x>+(x−t <t,x>

)cosϕ+t×xsinϕ.

The rotation is a linear transformationRot(t,ϕ)whose matrix isRot(t,ϕ):

x^rot=Rot(t,ϕ)x=xcosϕ+t <t,x>(1−cosϕ) +t×xsinϕ, (1.21) Rot(t,ϕ) =C_ϕI+ (1−C_ϕ)[t◦t] +S_ϕ[t×], (1.22)

which is the so-calledRodrigues formula[19]. Using the matrices of the dyadic product [t◦t] =t·t^T and the vector product[t×]it yields

Rot(t,ϕ)=





C_ϕ+ (1−C_ϕ)t_xt_x (1−C_ϕ)t_xt_y−S_ϕt_z (1−C_ϕ)t_xt_z+S_ϕt_y (1−C_ϕ)t_xt_y+S_ϕt_z C_ϕ+ (1−C_ϕ)t_yt_y (1−C_ϕ)t_yt_z−S_ϕt_x (1−C_ϕ)t_xt_z−S_ϕt_y (1−C_ϕ)t_yt_z+S_ϕt_x C_ϕ+ (1−C_ϕ)t_zt_z



. (1.23) The inverse of the rotation is the rotation aroundt by the angle −ϕ. Since[t◦t]is symmetric,[t×]is skew-symmetric, cos(−ϕ) =cos(ϕ)and sin(−ϕ) =−sin(ϕ)hence

Rot(t,ϕ)]⁻¹= [Rot(t,ϕ)]^T. (1.24) Therefore in case of orthonormed coordinate systems the inverse of the orientation matrixRis the transpose of the matrix R from which it follows that the inverse of the homogeneous matrixT can be easily computed:

R⁻¹=R^T, (1.25)

T =

R p 0^T 1

−1

=

R^T −R^Tp

0^T 1

. (1.26)

The orientation partRin the homogeneous transformationT contains 3×3=9 ele- ments. However they are not independent becausei⁰,j⁰,k⁰is an orthonormed basis therefore ki⁰k=kj⁰k=kk⁰k=1 and<i⁰,j⁰>=<i⁰,k⁰>=< j⁰,k⁰>=0, which are 6 conditions for the 9 elements. Thus the orientation can be described by 9−6=3 free parameters. In the case of Rodrigues formula these aret andϕ wheret is a unit vector having only 2 free parameters.

In the navigation practice of vehicles the names of the angles for z and x rota- tions are exchanged in the transformation RPY and simply called Euler angles:R_K,K0= RPY(ψ,ϑ,ϕ) which can easily be determined. It is also a common practice to use R^T_K,K0=RPY^T(ψ,ϑ,ϕ) =:S(ϕ,ϑ,ψ)but in this caseStransforms vectors from K into K⁰. Notice that in the navigation practice ϕ is the roll angle and ψ is the yaw angle, respectively.

1.4.4 Kinematic Model Using Euler (RPY) Angles

In case of Euler (RPY) angles we can parametrize the relative orientation by the rotation anglesΨ,Θ,Φaround thez,y,xaxes, respectively:

R_KI,KB = Rot(z,Ψ)Rot(y,Θ)Rot(x,Φ)

=





C_Ψ −S_ψ 0 S_Ψ C_Ψ 0

0 0 1









C_Θ 0 S_Θ

0 1 0

−S_Θ 0 C_Θ









1 0 0

0 C_Φ −S_Φ 0 S_Φ C_Φ





=





C_Ψ −S_ψ 0 S_Ψ C_Ψ 0

0 0 1









C_Θ S_ΘS_Φ S_ΘC_Φ

0 C_Φ −S_Φ

−S_Θ C_ΘS_Φ C_ΘC_Φ



⇒

=





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_Φ



. (1.27)

10 1.4 Orientation Description by Rotations and Quaternion SinceΨ,Θ,Φcan be considered as the joint variables of an RRR fictitious robot arm hence the relation between the angular velocityω =ω_xi_B+ω_yj_B+ω_zk_B and the joint velocities Ψ,˙ Θ,˙ Φ˙ can be used, but in the formula the joints axes aret₁=kfor ˙Ψ,t₂= jfor ˙Θand t₃=ifor ˙Φ, respectively. For the computation of the partial angular velocities the partial products are already present in the above formulas, hence we can select the appropriate row and write it in the appropriate column of the “Jacobian”:



 ω_x ωy

ω_z



 =





−S_Θ 0 1 C_ΘS_Φ C_Φ 0 C_ΘC_Φ −S_Φ 0







 Ψ˙ Θ˙ Φ˙



.

The determinant of the “Jacobian” matrix is det=−C_Θ and its inverse is [ ]⁻¹ = − 1

C_Θ





0 −S_Φ −C_Φ

0 −C_ΘC_Φ C_ΘS_Φ

−C_Θ −S_ΘS_Φ −S_ΘC_Φ



=





0 S_Φ/C_Θ C_Φ/C_Θ

0 C_Φ −S_Φ

1 T_ΘS_Φ T_ΘC_Φ



. Assume that the position of the COG isr_c=X_ci_I+Y_cj_I+Z_ck_I in the basis ofK_I. Then using the chosen parametrizationω =Pi_B+Q j_B+Rk_B, v=U i_B+V j_B+W k_B and the order ˙Φ,Θ,˙ Ψ˙ we obtain the following kinematic equations:



 X˙_c Y˙_c Z˙_c



 = R_KI,KB(Φ,Θ,Ψ)



 U V W



, (1.28)



 Φ˙ Θ˙ Ψ˙



 =





1 T_ΘS_Φ T_ΘC_Φ

0 C_Φ −S_Φ

0 S_Φ/C_Θ C_Φ/C_Θ







 P Q R



=:F(Φ,Θ)ω. (1.29)

1.4.5 Kinematic Model Using Quaternion

The critical vertical pose of the rigid body at Θ =±π/2 is singular configuration in Euler (RPY) angles hence some applications prefer the use of quaternion for orientation description.

The quaternion is the pairq= (s,w)∈R¹×R³consisting of a scalarsand a vectorw.

The conjugate is ˜q= (s,−w)and the norm is||q||=s²+w^Tw. In the setQof quaternions scalar multiplication and addition is defined as usual. The quaternion product is a new operation:

q₁⊗q₂= (s₁,w₁)⊗(s₂,w₂) = (s₁s₂−w^T₁w₂,w₁×w₂+s₁w₂+s₂w₁) (1.30) It follows from the Rodrigues formula thatRot(t,ϕ),ktk=1, can be identified by an appropriately chosen unit quaternionq:

Rot(t,ϕ) =C_ϕI+ (1−C_ϕ)[t◦t] +S_ϕ[t×]↔q= (C_ϕ/2,tS_ϕ/2), (1.31) q∗(0,r)∗q˜= (0,Rot(t,ϕ)r). (1.32) On the other handt²=<t,t>=1 hence<t⁰,t>+<t,t⁰>=0⇒t⊥t⁰thust,t×t⁰,t⁰is

an orthogonal basis andω =λ₁t+λ₂t×t⁰+λ₃t⁰. Notice that prime denote time derivative here.

Lemma 1.1.ω =ϕ⁰t+ (1−C_ϕ)t⁰×t+S_ϕt⁰.

Lemma 1.2.If q= (s,w) = (C_ϕ/2,tS_ϕ/2), kqk=1then ω =2 −ws⁰+ (−[w×] +sI)w⁰

. (1.33)

The proofs of the above two lemmas can be found in [15] and [18]

Remarks:

1. Sincekqk²=s²+w²=1⇒2(w^Tw⁰+ss⁰) =0 hence ω

0

=2

−w −[w×] +sI

s w^T

s⁰ w⁰

. (1.34)

2. The matrix in the above equation is orthogonal:

−w^T s [w×] +sI w

−w −[w×] +sI

s w^T

=I. (1.35)

3. The time derivative of the quaternion can be expressed byω andqin bilinear form:

s⁰ w⁰

= 1 2

−w^T s [w×] +sI w

ω 0

= 1 2

−ω^Tw ωs−ω×w

, s⁰

w⁰

=−1 2

0 ω^T

−ω [ω×]

s w

, (1.36)

s⁰ w⁰

=1 2

−w^T sI+ [w×]

ω. (1.37)

1.4.6 Conversion Between Orientation Descriptions

1) Sincea×(b×c) =b<a,c>−c<a,b>and for||t||=1 yieldst×(t×x) = [tt^T−I]x hencett^T = [t×]²+I and by (1.31) and substitution into the the series form of the complex exp(z)function follows

Rot(t,ϕ) =I+ (1−C_ϕ)[t×]²+S_ϕ[t×] (1.38) Rot(t,ϕ) =exp([t×]ϕ) (1.39) 2) If the unit quaternionq= (s,w) is known thenϕ can be determined from sand the

rotation axist fromwandRfrom the Rodrigues formula.

3) However this two level computation is superfluous. It follows from (1.32) that if the unit quaternionq= (s,w)is known then the rotation matrix can be immediately determined from it:

R:=I+2s[w×] +2[w×][w×]. (1.40)

12 1.4 Orientation Description by Rotations and Quaternion 4) The differential equation for Euler (RPY) angles and quaternion have already been discussed above but the case of DCM form is not considered although it may be important for modern control approaches. Let us consider now the case whenK₁and K₂are two orthogonal frames, the relative orientation isR_1,2and the relative angular velocity ofK₂with respect toK₁letω_1,2² expressed in the basis ofK₂. Then the time derivative of the rotation matrix can be derived by using the differentiation rule in moving coordinate system in the following form:

R⁻¹_1,2dR_1,2

dt = [ω_1,2² ×] (1.41)

from which follows ˙R_1,2=R_1,2[ω_1,2² ×].

Chapter 2

Control of Quadrotor UAVs

In this section I present an overview of the Quadrotor UAV concept. The research field of quadrotor unmanned aerial vehicles (UAVs) is highly prospering these days. Although the field was mainly motivated by possible military applications, civilian usage is emerg- ing quickly. Several military and civilian professionals are interested in developing an autonomous mini unmanned outdoor quadrotor helicopter. The benefits of such a system are significant, in the near future the quadrotor may be able to precisely follow a predefined path while performing a measurement series such as surveillance above a predefined terri- tory. In the control of quadrotor helicopters, several control algorithms can be considered, including linear and nonlinear algorithms of soft-computing algorithms. In the domain of nonlinear control algorithms, the most popular technique is the backstepping approach, although several other methods are elaborated including sliding mode [1] and feedback linearization control algorithms [2]. A full state backstepping algorithm is presented in [3].

These pieces of research not only differ from each other on the control algorithm, but also on the types of simplification of the dynamic model of the helicopter. Several methods exist for dynamic models that retain the basic behavior of the vehicle. Some neglect the rotor dynamics assuming the transients of the rotors are fast compared to those of the helicopter, some others do not consider the aerodynamics or the gyroscopic effect.

2.1 Dynamic model of quadrotors

2.1.1 Kinematic model of aerial vehicles

Let us assume that a frame (coordinate system)K_E fixed to the Earth can be considered as an inertial frame of reference. In our case it may be the NED frame considered to be fixed.

The frame fixed to the center of gravity of the helicopter K_H can be described by its position ξ = (x,y,z)^T and orientation (RPY angles) η = (Φ,Θ,Ψ)^T relative to K_E. Translational and angular velocitiesvandω of the helicopter are given inK_H.

The orientation can be described by the (orthonormal) matrixR_t in the following way:

R_t =





C_ΘC_Ψ S_ΦS_ΘC_Ψ−C_ΦS_Ψ C_ΦS_ΘC_Ψ+S_ΦS_Ψ C_ΘS_Ψ S_ΦS_ΘS_Ψ+C_ΦC_Ψ C_ΦS_ΘS_Ψ−S_ΦC_Ψ

−S_Θ S_ΦC_Θ C_ΦC_Θ



. (2.1)

The relation between ˙ξ and ˙η and translational and angular velocitiesvandω of the

14 2.1 Dynamic model of quadrotors helicopter take the form

ξ˙ =R_tv, ω =R_rη˙ (2.2)

where the (non-orthonormal) matrixR_r and its inverse are R_r =





1 0 −S_Θ

0 C_Φ S_ΦC_Θ 0 −S_Φ C_ΦC_Θ



,R⁻¹_r =





1 S_ΦT_Θ S_ΦT_Θ

0 C_Φ −S_Φ

0 S_Φ/C_Θ C_Φ/C_Θ



, (2.3)

and the derivative ofω can be written as

ε =ω˙ =R_rη¨+R˙_rη.˙ (2.4) Notice that the kinematic model is similar for both fixed wing and quadrotor UAVs.

2.1.2 Dynamic model

The concept of the quadrotor helicopter can be seen in Fig. 2.1. The helicopter has four actuators (four brushless DC motors) which exert lift forces f_iproportional to the square of the angular velocitiesΩ_iof the actuators. The rotational directions of the propellers are shown in the sketch.

f

f f

f

1 3

4

2

f

x

y

Ù

Ù

Ù

Ù

Fig. 2.1. Concept of the quadrotor helicopter

Applying Newton’s laws, the translational and rotational motions of the helicopter in K_H are described by

∑

∑

(2.5) whereI_c is the inertia matrix of the helicopter and it is supposed that it can be described by a diagonal matrixI_c=diag(I_x,I_y,I_z).∑F_ext and∑T_ext represent the forces and torques

respectively applied to the quadrotor helicopter expressed inK_H. These forces and torques are partly caused by the rotation of the rotors (F andT), the aerodynamic friction (F_aand T_a), the gravitational effect (F_g) in the translational motion and the gyroscopic effect (T_g) in the rotational motion:

∑

∑

(2.6) The helicopter has four actuators (four brushless DC motors) which exert a lift force proportional to the square of the angular velocitiesΩ_i of the actuators (f_i=bΩ²_i). The BLDC motors’ reference signals can be programmed inΩi. The resulting torque and lift force are

T =





lb(Ω²₄−Ω²₂) lb(Ω²₃−Ω²₁) d(Ω²₂+Ω²₄−Ω²₁−Ω²₃)



,

f = f₁+f₂+f₃+f₄=b

4 i=1

∑

Ω²_i,

(2.7)

where l, b, d are helicopter and rotor constants. The force F can then be rewritten as F= (0,0,f)^T.

The gravitational force points to the negative z-axis, hence F_g = −mR_t^T(0,0,g)^T

=−mR^T_t G. The gyroscopic effect can be modeled as

T_g=−(ω×k)I_r(Ω₂+Ω₄−Ω₁−Ω₃) =−ω×(I_rΩ_r) (2.8) whereI_ris the rotor inertia andkis the third unit vector.

The aerodynamic friction at low speeds can well be approximated by the linear formulas F_a=−K_tvandT_a=−K_rω.

Using the equations above we can derive the motion equations of the helicopter:

F=mR^T_t ξ¨−K_tR^T_t ξ˙−mR^T_t G, T =I_cR_rη¨+I_c

∂R_r

∂Φ

Φ˙ +∂R_r

∂Θ Θ˙

η+˙ +K_rR_rη˙ + (R_rη˙)×(I_cR_rη˙ +I_rΩ_r).

(2.9)

2.1.3 Simplified dynamic model

A simplified model of the quadrotor helicopter can be obtained by neglecting certain effects and applying reasonable approximations. For slow helicopter motion it is reasonable to neglect all the aerodynamic effects, namely,K_t andK_r are approximately zero matrices.

Slow motion in lateral directions means little roll and pitch angle changes, thereforeR_r can be approximated by a 3-by-3 unit matrix. Such simplification cannot be applied toR_t.

Consequently, the dynamic equations in (2.9) become F≈mR^T_t ξ¨−mR^T_t G,

T ≈I_cη¨+η˙×(I_cη˙ +I_rΩ_r). (2.10)