Adaptive Rate Control for Multirotor UAVs - Timo Hinzmann

Autonomous Systems Lab Prof. Roland Siegwart

Semester Thesis

Adaptive Rate Control for Multirotor UAVs Autumn Term 2014

Supervised by: Michael Burri Markus Achtelik Sammy Omari

Author: Timo Hinzmann

Declaration of Originality

I hereby declare that the written work I have submitted entitled Adaptive Rate Control for Multirotor UAVs is original work which I alone have authored and which is written in my own words.1

Author(s) Timo

Hinzmann

Supervising lecturer Michael Markus Sammy

Burri Achtelik Omari

With the signature I declare that I have been informed regarding normal academic citation rules and that I have read and understood the information on ’Citation etiquette’ (http://www.ethz.ch/students/exams/plagiarism_s_en.pdf). The citation conventions usual to the discipline in question here have been respected. The above written work may be tested electronically for plagiarism.

Place and date

Signature

1 Co-authored work: The signatures of all authors are required. Each signature attests to the originality of the entire piece of written work in its final form.

Contents Preface

vii

Abstract

ix

1 Introduction

3

2 Hardware and software setup 2.1 Hardware . . . . . . . . . . . . . . . . . . 2.1.1 Low-level and high-level processor 2.2 Software . . . . . . . . . . . . . . . . . . . 2.2.1 AscTec Simulink SDK . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

5 5 5 6 6

3 Quadrocopter dynamics 3.1 Newton-Euler approach 3.2 Rotational dynamics . . 3.3 Control allocation . . . 3.4 Actuator dynamics . . . 3.5 Plant . . . . . . . . . . . 3.6 Parameters . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

9 9 9 11 12 13 14

4 Adaptive control 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . 4.2 Approaches . . . . . . . . . . . . . . . . . . . . . 4.2.1 Model reference adaptive control (MRAC) 4.2.2 Self-tuning control (STC) . . . . . . . . . 4.2.3 Composite adaptation . . . . . . . . . . . 4.3 Online parameter estimation . . . . . . . . . . . 4.3.1 Remarks . . . . . . . . . . . . . . . . . . . 4.4 Stability and robustness . . . . . . . . . . . . . . 4.4.1 Parametric uncertainty . . . . . . . . . . 4.4.2 Non-parametric uncertainty . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

17 17 17 17 18 19 19 20 21 21 22

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

5 Adaptive control of angular rates and online inertia estimation 5.1 Derivation: Adaptation and control law . . . . . . . . . . . . . . . . 5.2 Stability and convergence analysis . . . . . . . . . . . . . . . . . . . 5.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 Case I: Parametric uncertainty . . . . . . . . . . . . . . . . . 5.3.2 Case II: Parametric uncertainty and actuator dynamics . . . 5.3.3 Case III: Parametric uncertainty, actuator dynamics and measurement noise . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.4 Bursting and deadzone modification . . . . . . . . . . . . . . 5.3.5 Composite adaptation . . . . . . . . . . . . . . . . . . . . . . 5.4 Implementation results . . . . . . . . . . . . . . . . . . . . . . . . . . ii

25 25 27 30 30 32 34 36 38 40

6 Thrust control and online 6.1 Batch least squares . . . 6.2 Recursive least squares . 6.3 Simulation results . . . 6.4 Implementation results .

mass estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

43 43 44 45 46

and inertia estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

49 49 49 50 51 51 51

A Math A.1 Euler angles and angular velocity . . . . . . . . . . . . . . . . . . . . A.2 Rotation matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

53 53 53

Bibliography

56

7 Conclusion and outlook 7.1 Adaptive control of angular body rates 7.1.1 Discussion . . . . . . . . . . . . 7.1.2 Future works . . . . . . . . . . 7.2 Mass estimation and thrust control . . 7.2.1 Discussion . . . . . . . . . . . . 7.2.2 Future works . . . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

List of Figures 1.1

AscTec Hummingbird . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.1 2.2 2.3 2.4 2.5

AscTec Hummingbird and Futaba remote control AscTec AutoPilot board . . . . . . . . . . . . . . onboard_matlab.mdl . . . . . . . . . . . . . . . . onboard_matlab\Onboard_Matlab_Controller . UART_communication.mdl . . . . . . . . . . . .

. . . . .

5 6 7 7 8

3.1 3.2 3.3 3.4 3.5 3.6 3.7

Body coordinate frame, moments and motor speeds . . . . . . . . . . Control allocation . . . . . . . . . . . . . . . . . . . . . . . . . . . . Actuator dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . Actuator model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Experimential identification of inertia . . . . . . . . . . . . . . . . . Calculation of inertia: Modelling motors and quadrocopter body as cylinders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

10 12 13 13 13 15

4.1 4.2 4.3

Model reference adaptive control (MRAC) . . . . . . . . . . . . . . . Self-tuning control (STC) . . . . . . . . . . . . . . . . . . . . . . . . Projection operator implementation . . . . . . . . . . . . . . . . . .

18 18 23

5.1 5.2 5.3

28 30

5.9 5.10 5.11 5.12 5.13 5.14

3-D plot for Lyapunov candidate V and total derivative −V˙ . . . . . Case I: Parametric uncertainty . . . . . . . . . . . . . . . . . . . . . Simulation results case I: Parametric uncertainty with low adaptation gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation results case I: Parametric uncertainty with high adaptation gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Case II: Parametric uncertainty and actuator dynamics . . . . . . . Simulation results case II: Parametric uncertainty and actuator dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Case III: Parametric uncertainty, actuator dynamics and measurement noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation results case III: Parametric uncertainty, actuator dynamics and measurement noise . . . . . . . . . . . . . . . . . . . . . . . . Bursting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Dead-zone modification . . . . . . . . . . . . . . . . . . . . . . . . . Composite adaptation . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation results: Composite adaptation . . . . . . . . . . . . . . . Implementation setup: Inertia convergence . . . . . . . . . . . . . . . Implementation results: Inertia convergence and angular velocity . .

6.1 6.2

Feedforward control of thrust . . . . . . . . . . . . . . . . . . . . . . Simulation results: Mass estimation . . . . . . . . . . . . . . . . . .

43 45

5.4 5.5 5.6 5.7 5.8

iv

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

15

31 32 33 33 34 35 36 37 38 40 41 41

6.3 6.4

Averaged values for kn using AscTec test-bench data . . . . . . . . . Implementation results: Mass estimation . . . . . . . . . . . . . . . .

46 47

7.1 7.2 7.3

Outlook: Quadrocopter equipped with aerial robotic manipulator . . Outlook: Pick up and drop payload . . . . . . . . . . . . . . . . . . . Outlook: Mass estimation RLS with forgetting factor . . . . . . . . .

51 52 52

Thanks to the Ph.D. students and Postdocs from the Autonomous Systems Lab (ASL) for their support and advice during my semester project - in particular I want to thank my supervisors Michael Burri, Markus Achtelik and Sammy Omari.

vii

Abstract In this semester project a low-level adaptive controller is designed that commands angular rates ω based on a desired reference ωdes calculated by a high-level controller and gyroscope measurements ωmeas . The controller ensures stability of the system, convergence of the tracking error and estimates the inertia matrix J in real-time onboard of the UAVs low-level processor. In addition to commanding the angular rates, the controller estimates the mass and calculates the desired thrust based on the reference signal. The controller handles non-parametric uncertainties such as measurement noise, unmodeled actuator dynamics or additional payload. The ultimative goal of this semester project is to design one modular adaptive angular rate controller for changing platforms and different types of multirotor UAVs, that is for quadrocopters and hexacopters.

ix

1

Abbreviations Abbreviation

Description

JTAG

Joint Test Action Group

HL

High Level processor

LL

Low Level processor

LiPo

Lithium-ion Polymer batteries

DC

Direct Current

SDK

Software Development Kit

IMU

Inertial Measurement Unit

UART

Universal Asynchronous Receiver Transmitter

CPU

Central Processing Unit

MRAC

Model Reference Adaptive Control

STC

Self Tuning Control

RLS

Recursive Least Squares

AscTec

Ascending Technologies, UAV manufacturer

XBee

Radio module manufacturer

GPS

Global Positioning System

Symbols Symbol

Unit

φ

rad

roll angle

θ

rad

pitch angle

ψ

rad

yaw angle

J

kgm2 2

Jp

kgm

Jr

kgm2

Description

inertia (around the UAV mass) inertia (around the pendulum) rotor inertia

Nm

body moment

L

Nm

body moment around x-axis (Roll moment)

M

Nm

body moment around y-axis (Pitch moment)

N

Nm

body moment around z-axis (Yaw moment)

τ = [L, M, N ]

T

2

Symbol

Unit

Description

rad s

body angular velocity

p

rad s

body angular velocity around x-axis (Roll rate)

q

rad s

body angular velocity around y-axis (Pitch rate)

r

rad s

body angular velocity around z-axis (Yaw rate)

ω ˜

rad s

angular velocity tracking error

ωref

rad s

angular velocity reference model

ωmeas

rad s

measured angular velocity

v

m s

kn

N RP M 2

thrust constant

km

m

motor constant

l

m

UAV arm length

mmotor

kg

motor mass

rmotor

m

radius of motor

hmotor

m

height of motor

m0

kg

mass of UAV body

r0

m

radius of UAV body

h0

m

height of UAV body

A

−

allocation matrix

Fi

Nm

n

RP M

K

−

controller gain

Γ

−

adaptation gain

θˆ

−

parameter estimate

∆

−

system output vector

∆ˆ

−

predicted system output vector

∆DZ

−

dead-zone width

λ

−

forgetting factor

V

−

Lyapunov candidate

V˙

−

total derivative of Lyapunov candidate

P

−

variance

ω = [p, q, r]

T

velocity

thrust of rotors rotor speed

Chapter 1

Introduction

Figure 1.1: AscTec Hummingbird

Motivation This semester project is motivated by the experience at the Autonomous Systems Lab (ASL) that for changing UAV platforms the system parameters need to be identified and the controller needs to be retuned to achieve decent performance. In particular, the diagonal elements of the inertia matrix and the UAV mass are of special interest for the controller. For instance, knowledge of the UAV mass is needed to command the correct thrust to the motors. The goal of this semester project is to design a controller that is able to adapt to different conditions and automatically estimates the system parameters of the mass and inertia and sends it to the ground station in real-time. Outline The thesis is structured as follows: In chapter 2 the hardware and software that is used for the project is presented. A modified version of the AscTec Hummingbird is used as a test platform 1.1 in the ASL flying room. A short overview of the AscTec Simulink Software Development Kit (SDK) is given and otherwise referred to the AscTec manual1 . In chapter 3 the dynamics of a quadrotor are derived and the system parameters that need to be identified are introduced. The command allocation from the controller via the actuator dynamics and to the plant is shown in detail. Chapter 4 gives a quick overview about adaptive control and compares the three 1 See

[1] and [2] for further information

3

Chapter 1. Introduction

4

basic approaches: model reference adaptive control (MRAC), self-tuning control (STC) and composite adaptation. The chapter presents the possible estimation schemes used in the three adaptive control architectures. Basic theorems and statements on stability, asymptotic tracking and parameter convergence in adaptive control are stated and shortly explained. The chapter concludes with common modifications used to make the controller robust against nonparametric noise. In chapter 5 the algorithm to control the angular rates is presented and it is shown how the inertia can be estimated simultaneously. Chapter 6 presents the thrust control and online mass estimation. The semester project concludes with a discussion in chapter 7 in which results as well as advantages and disadvantages of the implemented approaches are discussed. Finally, future works are proposed to continue the work implemented in this semester project.

Chapter 2

Hardware and software setup 2.1

Hardware

The hardware components provided by AscTec consist of • AutoPilot board with low-level and high-level processor • Brushless DC motors with individual motor controllers • Futaba remote control (see figure 2.1) and receiver • Xbee receiver/transmitter • LiPo battery pack Further information can be found in [1].

Figure 2.1: Left: AscTec Hummingbird with roll, pitch and yaw axis. Right: Futaba remote control [1]

2.1.1

Low-level and high-level processor

"The low-level processor is the heart of the vehicle running all critical functions as sensing and filtering, attitude estimation and flight control. The code on this processor is protected and cannot be accessed using a JTAG interface nor be read through the serial interface." 1 . The user’s C-code needs to be uploaded to the High-level controller using a JTAG adapter mounted on the HL bootloader jumper pads shown in figure 2.2. To 1 From

the AscTec manual [1]

5

Chapter 2. Hardware and software setup

6

wirelessly communicate with the ground station the quadrocopter uses Xbee receiver/transmitter. It is possible to use one Xbee device for receiving and transmitting or, in order to increase data transfer rates, to use two for receiving and transmitting respectively 2 . For testing purposes the HL serial 0 port can simply be connected to the ground station via a serial interface.

Figure 2.2: AscTec AutoPilot 3 , the JTAG adapter needs to be mounted on the HL bootloader jumper pads for code upload, the Xbee receiver or serial interface needs to be connected to the HL serial 0 port to receive and send data.

2.2

Software

The user’s code can either be manually incorporated in the AscTec C-code or implemented in the AscTec Simulink SDK. The latter approach is described in the following subsection.

2.2.1

AscTec Simulink SDK

The AscTec Software includes a Simulink SDK4 (Simulation Development Kit) that can be converted to C-Code using the Simulink Coder (previously called Real Time Workshop) and flashed onto the High-level processor using Eclipse. The SDK consists of two main Simulink models: onboard_matlab.mdl and UART_communication.mdl.

Onboard model The top layer of onboard_matlab.mdl can be seen in figure 2.3. Sensor data (IMU and GPS) as well as data from the remote control and UART interface are inputs to the model. The incoming data is converted in the respective Signal_mapping subsystems and sent to the Onboard_Matlab_Controller where the customized controller can be placed. The controller consists of a command filter, an attitude controller and a control allocation (cf. figure 2.4). The command filter converts the remote command to appropriate values 5 . As default, a proportional attitude and rate controller are 2 [3] 4 [2] 5 Refer

to [2] for more information

7

2.2. Software

Figure 2.3: onboard_matlab.mdl: This provided by the AscTec Simulink SDK uses the sensor data to compute the control output and to command the motors

implemented. The control allocation maps the moments and thrust to motor RPM by basically inverting equation 3.13.

Figure 2.4: onboard_matlab\Onboard_Matlab_Controller: As default, a P-controller for attitude and angular rates is provided by the AscTec Simulink SDK

The control variables computed by the controller are mapped to appropriate values and sent to the motor (cf. figure 2.3). The UART data output is used to send data to UART_communication.mdl during flight.

UART communication model UART_communication.mdl is used to receive data (e.g. IMU measurements) in real-time from the quadrotor and to send data from the ground station to the quadrotor (e.g. to tune the controller during flight). The top layer in figure 2.5 displays the CPU load and battery voltage. Further information about the AscTec Simulink SDK is presented in [2],[3] and [4]. Information about the usage of fixed-point datatypes in Simulink can be found in [5].

Chapter 2. Hardware and software setup

8

Figure 2.5: UART_communication.mdl: The UART communication model is used to receive sensor data from the UAV and to send parameters or control data to the UAV in real-time

Chapter 3

Quadrocopter dynamics The chapter first discusses the Newton-Euler equations to derive the Quadrocopter dynamics. In section 3.2 the rotational dynamics of the Euler equation are applied to a quadrocopter. The chapter concludes with a detailed description of the control allocation from the controller to the plant.

3.1

Newton-Euler approach

The quadrocopter dynamics can be derived using the Newton-Euler equations which combine the translational and rotational dynamics of a rigid body. These equations are valid with respect to a body reference frame which coincides with that point of the rigid body where the mass is centered, i.e. the center of gravity.

m · I3×3 0

0 J

v˙ ω × mv F + = ω˙ ω × Jω τ

(3.1)

This project focuses primarily on the rotational dynamics of a rigid body which is described by the second equation (Euler equation). The next section explains what body moments τ can be generated by a multirotor UAV to achieve a desired angular velocity.

3.2

Rotational dynamics

The Euler equation describing the rotational dynamics is given by J · ω˙ + ω × J · ω = τ

(3.2)

ω˙ = J −1 (τ − ω × J · ω)

(3.3)

which is equivalent to:

The left side of the equation is the system output which is a result of a rotating rigid body described by the cross terms ω × J · ω and the moments τ induced by the four propellers. The considered moments τ are:

9

Chapter 3. Quadrocopter dynamics

10

𝒏𝟏

𝒏𝟒

𝒙

𝒏𝟑 𝒏𝟐

𝑙

𝒚

𝑴 𝑵

L

𝒛

Figure 3.1: Body coordinate frame x ni

y

z

T

, moments τ = L

M

N

T

and motor speeds

Propeller gyroscopic effect The gyroscopic effect introduced by the four rotating propellers with rotor inertia Jr is usually modeled as 1    4 0 X τ1 = Jr ω × 0 (−1)i ni (3.4) i=1 1 where ni with i ∈ [1, 2, 3, 4] describes the speed of the individual rotors shown in figure 3.1. Equation 3.4 is equivalent to L1 = −Jr q(−n1 + n2 − n3 + n4 )

(3.5)

M1 = −Jr p(−n1 + n2 − n3 + n4 )

(3.6)

with Jr : rotor inertia p : angular velocity around the x-axis q : angular velocity around the y-axis ni : speed of individual rotors

Actuator effect If all propellers spin with the same speed, i.e. during hover, no moments around the x and y axis are introduced. A difference in the thrust of 1 cf.

[6]

11

3.3. Control allocation

opposing propellers is used to generate a roll moment L around the x axis and a pitch moment M around the y axis: L2 = lkn (n24 − n22 )

(3.7)

lkn (n21

(3.8)

M2 =

−

n23 )

hereby kn : thrust coefficient l : quadrocopter arm length

Counter-torque unbalance Two opposing propellers spin clockwise while the other two spin counter-clockwise. A non-zero moment around the z-axis remains if the counter-torques are not balanced: N1 = km (−n21 + n22 − n23 + n24 )

(3.9)

with km : motor constant, drag coefficient

Sum of moments The total moments around the individual axes is found by summing up the moments explained above. The propeller gyroscopic effect is usually small since it depends on ni (instead of n2i ) and due to a small motor inertia Jr and thus can be neglected: X L= Li = −Jr q(−n1 + n2 − n3 + n4 ) + lkn (n24 − n22 ) ≈ lkn (n24 − n22 ) (3.10) i

M=

X

N=

X

Mi = −Jr p(−n1 + n2 − n3 + n4 ) + lkn (n21 − n23 ) ≈ lkn (n21 − n23 ) (3.11)

i

Ni = km (−n21 + n22 − n23 + n24 )

(3.12)

i

L 0 τ = M  =  lkn N −km 





−lkn 0 km

0 −lkn −km

   n21 lkn  2  n2  0  n23  km n24

(3.13)

with kn : thrust coefficient km : motor constant, drag coefficient l : quadrocopter arm length

3.3

Control allocation

Mapping I

The equation for the angular velocity is J ω˙ = τ − ω × (Jω)

(3.14)


Controller

  p˙  q˙     r˙  T

12



 L M    N  T

  F1 F2    F3  F4

I

II

  n1 n2    n3  n4 III

Actuator Dynamics

Figure 3.2: Control allocation

The above equation can be approximated by J ω˙ ≈ τ

(3.15)

by neglecting the cross terms. Using τ = [L, M, N ]T and ω = [p, q, r]T and assuming a diagonal inertia matrix J 2 we get the mapping     L Jxx p˙ M  ≈  Jyy q˙  (3.16) N Jzz r˙ Mapping II The motor thrust Fi and the body moments τ are connected using the allocation matrix A. The allocation matrix depends on the length of the quadrocopter arm l and the motor constant km and can be calculated as shown in section 3.6. The mapping is then defined by      F1 L 0 F2  = A−1 M  =  l F3 N −km

−l 0 km

0 −l −km

−1   l L 0  M  km N

(3.17)

The collective thrust T is usually commanded by the pilot and can be equally distributed on the propellers (cf. [4]). The complete mapping is then found by plugging in numerical values, e.g. l = 0.17 m, km = 0.016 m 3      F1 0 2.9412 −15.625 0.25 L F2  −2.9412  M  0 15.625 0.25  =   (3.18) F3   0 −2.9412 −15.625 0.25   N  F4 2.9412 0 15.625 0.25 T Mapping III The relation of the thrust vector Fi and the squared motor speeds ni is assumed to be linear: Fi = kn n2i

(3.19)

The inverse mapping is then ni =

3.4

r

1 Fi kn

(3.20)

Actuator dynamics

The desired motor speed ni,des , i ∈ {1, ..., 4} of the propellers is calculated by the controller and commanded to the actuator. The response of the actuator can be modeled fairly accurately by a first order system (PT1 element) with a gain K

13

3.5. Plant

  n1,c n2,c    n3,c  n4,c

  n1 n2    n3  n4

Actuator Dynamics Figure 3.3: Actuator dynamics

nmodel , nmeas [RPM]

and a time constant T . The actual speed of the rotors for a desired speed ni,des is measured and compared with the model. As depicted in figure 3.4 a high accuracy was achieved with a time constant of T = 0.05 s and a gain of K = 0.9. Rotor angular speed n 2,200 2,000 1,800 1,600 1,400 39.4

nmodel nmeas

39.6

39.8

40

40.2

40.4

40.6

40.8

41

41.2

Time t [s] Figure 3.4: Measured motor speed compared to predicted motor speed by first order system for a given desired motor speed

3.5

Plant   n1 n2    n3  n4

III −1

  F1 F2    F3  F4

  p˙  q˙     r˙  T



 L M    N  T II −1

I −1

R

  p q  r

Figure 3.5: Plant

The inverse mappings can be derived by inverting the equations in section 3.3: Mapping III −1

The inverse is given in equation 3.19 with Fi = kn n2i

Mapping II −1 

    L F1 0 M  F2   l   = A  =  N  F3  −km T F4 0.25

−l 0 km 0.25

(3.21)

0 −l −km 0.25

  l F1 F2  0    km  F3  0.25 F4

(3.22)

2 If one assumes that the "body axes are close to the principal axes" [7] than the off-diagonal terms in the inertia matrix can be neglected 3 The values are taken from [4] where an AscTec Hummingbird was used


14

Mapping I −1         p˙ L p p q˙ = J −1 M  − q  × J q  r˙ N r r Complete plant equation   p˙ q˙ =J −1 (τ − ω × Jω) r˙    −1   F1 Jxx 0 0 0 −1 0 1   F2  −1 Jyy 0  1 0 1 0  = 0 −  F3  −1 −1 1 −1 1 0 0 Jzz F4  −1   Jxx 0 0 qr(Jzz − Jyy ) −1  0 Jyy 0  pr(Jxx − Jyy ) −1 pq(Jyy − Jxx ) 0 0 Jzz

(3.23)

(3.24)

(3.25)

(3.26)

Introducing the states x1 = p, x2 = q, x3 = r and ui = Fi with i = 1, 2, 3, 4 yields      −1   u1 Jxx 0 0 x˙ 1 0 −1 0 1   u2  −1 x˙ 2  =  0 Jyy 0  1 0 1 0  − (3.27)  u3  −1 x˙ 3 −1 1 −1 1 0 0 Jzz u4  −1   Jxx 0 0 x2 x3 (Jzz − Jyy ) −1  0 Jyy 0  x1 x3 (Jxx − Jyy ) (3.28) −1 x1 x2 (Jyy − Jxx ) 0 0 Jzz

3.6

Parameters

Intertia Several approaches can be used to obtain the diagonal elements of the inertia offline: • Experimental identification: In [4] a pendulum test bench shown in figure 3.6 was used to measure the period T which the quadrocopter needs to swing around the x, y and z axis. The inertia Jp around the pendulum is calculated using Jp =

mglp T 2 (2π)2

(3.29)

where m is the total mass of the quadrocopter and lp is the length of the pendulum (quadrocopter arm l plus length of pendulum adapter). The parallel axis theorem transforms the inertia Jp around the pendulum to the inertia J around the quadrocopter mass: J = Jp − mlp2

(3.30)

• Calculation: The inertia can for example be calculated by modeling the four motors and the quatrocopter’s body as cylinders as it is shown in figure 3.7.

15

3.6. Parameters

Figure 3.6: Experimential identification of inertia

Different approaches and assumptions are shown in [8], [9] and [10]. In [10] the diagonal elements of the inertia matrix are derived as 2 mmotor rmotor mmotor h2motor m0 r02 m0 h20 + + 2mmotor l2 + + (3.31) 2 6 4 12 = Jxx (3.32)

Jxx = Jyy

Jzz =

m0 r02 + 4mmotor l2 2

(3.33)

with mmotor : motor mass rmotor : radius of motor hmotor : height of motor m0 : mass of body r0 : radius of body h0 : height of body l : quadrocopter arm length

Figure 3.7: Calculation of inertia: Modelling motors and quadrocopter body as cylinders

• Estimation The diagonal elements of the inertia matrix can be estimated using flight data and applying an estimator such as least square. The simplified equations are Jxx p˙ ≈ kn l(−n22 + n24 ) Jyy q˙ ≈ Jzz r˙ ≈

− n23 ) kn km (−n21 +

(3.34) (3.35)

kn l(n21

n22

−

n23

+

n24 )

(3.36)

and are listed in table 5.1. Note that the equation depends on other parameters and on the derivative of the angular velocity.


Arm length

16

The arm length l as depicted in figure 3.1 can easily be measured.

Motor constant km data as shown in [4].

The motor constant km can be computed using test bench

Thrust constant kn section 6.4.

The thrust constant is measured during a hover flight in

Chapter 4

Adaptive control This chapter gives an overview about adaptive control architectures and is mainly based on classical books about nonlinear and adaptive control. For examples and proofs please refer to [11], [12] and [13].

4.1

Introduction

Adaptive control The goal of adaptive control is to estimate unknown plant parameters online and to compute a control input based on the updated parameters and the desired reference. Adaptive control was first developed in the 1950’s to automatically adapt to the changing parameters in aircrafts [11]. Robust control and adaptive control: A comparison Both robust control and adaptive control are used to control systems where one or multiple plant parameters are uncertain. In case of robust control bounds have to be defined within which the controller is guaranteed to keep a constant performance. In contrast, in adaptive control no bounds have to be defined, i.e. no knowledge about the parameters is needed. A further adavantage is that instead of keeping a constant performance, the adaptive controller is able to learn the parameters and to improve tracking performance.

4.2

Approaches

Adaptive controllers were originally classified into model reference adpative control and self-tuning control (cf. [11]). The disadvantages of the two approaches motivated a new, third approach: the composite adaptation.

4.2.1

Model reference adaptive control (MRAC)

Model reference adaptive control is an widely used concept in adaptive control. It aims to guarantee that the (non-)linear system behaves like a chosen reference model under the influence of uncertainty in the system. Model reference adaptive control consists of the following elements (cf. 4.1): • A plant of known structure with unknown parameters which are to be estimated • A reference model that encodes the desired transient response (such as rise time) and stability properties. The reference dynamics are constrained by 17

Chapter 4. Adaptive control

18

true plant dynamics, i.e. the system needs to be able to follow the reference model dynamics. • An adaptation law which ensures asymptotic convergence of tracking error y˜. • A control law that ensure stability and tracking convergence. The control law needs to be linearly parametrized with respect to unknown parameters.

ydes

y˜

Reference model

yref u

Control law

−

y

Plant

y˜

+

θˆ Adaptation law

Figure 4.1: Model reference adaptive control (MRAC) (based on[11])

4.2.2

Self-tuning control (STC)

The basic elements of self-tuning controllers, depicted in figure 4.2, are: • A plant of known structure but unknown parameters • A (recursive) parameter estimator that estimates the plant parameters. Possible algorithms are listed in section 4.3. • A controller that computes the control signal based on the reference and the estimated parameters. The fact that an estimated parameter can be used to compute the control signal instead of the true parameter is based on the certainty of equivalence (cf. [11]).

ydes

u

Control law

θˆ

Plant

y

Estimator

Figure 4.2: Self-tuning control (STC) (based on[11])

MRAC and STC: A comparison • MRAC adjusts the parameters such that the tracking error converges asymptotically to zero whereas the self-tuning controller estimates the plant parameters such that the error between the input and output data is minimized (e.g. in the sense of least squares).

19

4.3. Online parameter estimation

• A large variety of (recursive) estimators can be used in STC (cf. section 4.3). However, compared to MRAC, this makes it more difficult to guarantee stability and convergence.

4.2.3

Composite adaptation

The update law in model reference adaptive control is driven solely by the tracking error between the reference model and the measurements of the plant. However, pure tracking-error based approaches tend to show slow convergence of the parameters and oscillatory behavior especially when choosing high learning rates 1 . This motivated Duarte & Narendra in [14] and Slotine & Li in [11] to combine two sources of parameter information: the tracking error e and the prediction error ep . They called their architectures "Combined model reference adaptive control" and "Composite model reference adaptive control" respectively. They showed that combining prediction and tracking errors in the adaptive law leads to smoother transient behaviour than traditional MRAC architectures. This intuitive result is proven in the CMRAC conjecture [15]. Possible algorithms for calculating the prediction error are listed in section 4.3.

4.3

Online parameter estimation

Time-varying parameters such as the quadrocopter’s payload can be estimated online using recursive parameter estimation methods. The measurement equation needs to be linearly parametrized of the form 2 ∆ = θT Φ

(4.1)

where y is the system output, θ is the parameter vector and Φ is the regressor (measurement vector). In prediction-error based estimation methods one defines the predicted system output as ˆ = θˆT Φ ∆

(4.2)

The prediction error ep is then simply defined as ep = ∆ − ∆ˆ = (θT − θˆT )Φ = θ˜T Φ

(4.3)

˜ with the parameter estimation error θ. Recursive least-squares estimator (RLS) tions in the discrete case are 3

The recursive least-squares equa-

K = P (k − 1)Φ(k − 1)(R + Φ(k − 1)T P (k − 1)Φ(k − 1))−1 ˆ ˆ − 1) + K[∆(k) − ΦT (k)θ(k ˆ − 1)] θ(k) = θ(k

(4.4)

P (k) = (I − KΦ(k))P (k − 1)

(4.6)

with 1 [11],

382 ff. mainly based on [7] 3 Notation mainly based on [16] 2 Notation

(4.5)


K: P: Φ: R: ˆ θ: ∆:

20

Gain Variance Regressor (Measurement vector) Measurement noise Parameter estimate System output vector

Note that in equation 4.5 the parameter estimate is driven by the prediction error ep . The least-squares algorithm is known to be able to deal with high-frequency noise. However, it is not well suited to estimate fast changing parameters. Consider the task of estimating the mass of quadrotor that picks up and drops payload. The problem is that the RLS algorithm tries to fit the input/output data for all times, i.e. also for data that was generated by a different parameter. Recursive least-squares estimator with forgetting factor The above mentioned problem motivated a new RLS formulation in which new data has more weight. This is considered by introducing the forgetting factor λ: K = P (k − 1)Φ(k − 1)(λ + R + Φ(k − 1)T P (k − 1)Φ(k − 1))−1 ˆ ˆ − 1) + K[∆(k) − ΦT (k)θ(k ˆ − 1)] θ(k) = θ(k

(4.7)

P (k) = (I − KΦ(k))λ

(4.9)

−1

4.3.1

P (k − 1)

(4.8)

Remarks

For control design the parameters to be estimated are usually assumed to be constant. The adaptive controller is still applicable to time-varying parameters if the dynamics of the parameters are slower than the parameter recomputation. In order to estimate the unknown parameters they have to be linearly parametrized which is not always possible. However, as we will see in the next chapter it is possible for the case of inertia estimation. Parameter estimation and parameter convergence in adaptive control Most works on adaptive control ensure only that asymptotic tracking is achieved globally, i.e. the tracking error converges to zero. However, this does not mean that the parameter estimates converge to their true values. This fundamental problem of parameter estimation in adaptive control is based on the Persistent Excitation (PE) condition. This states that - when using current data - the adaptive weights converge to the true plant parameters if and only the states of the plant are persistently excited by the control input. For example, exponentially decreasing functions can be added to the input signal to satisfy the PE condition. In [17] it was shown that the estimates of a 3 × 3 inertia matrix of a 3-DOF rigid body converge to their true values by using periodic input signals. However, this leads to undesired wear of the actuator or infeasible system behaviour. More recent research aimed to ensure parameter convergence to their true values even when the plant states are not persistently excited. Girish V. Chowdhary showed in [16] that parameters are guaranteed to converge without persistent excitation when current and past data is used concurrently.

21

4.4. Stability and robustness

4.4

Stability and robustness

A challenge in adaptive control is to guarantee stability and asymptotic convergence of the tracking error. An often used approach is the Lyapunov theory which can prove stability of the system and tracking error convergence in systems influenced by parametric uncertainty, i.e. unknown plant parameters. However, it cannot handle non-parametric uncertainty such as measurement noise or disturbances. In the follwing sections, first parametric uncertainties and then non-parametric uncertainties are considered.

4.4.1

Parametric uncertainty

The Lyapunov theory is a powerful tool to analyse the stability properties of the equilibrium x∗ : Definition 1 (Lyapunov stability [18]) (a) If ∃ V (x), being LPDF, and V˙ (x) ≤ 0 ∀x ∈ Bh , for some h > 0 then x∗ = 0 is stable. (b) If ∃ V (x), being LPDF, and −V˙ (x) is LPDF, then x∗ is locally asymptotically stable. (c) If ∃ V (x), being PDF, and −V˙ (x) is PDF, then x∗ is globally asymptotically stable. where (L)PDF stands for (locally) positive definite and Bh defines a ball with radius h > 0. The Lyapunov candidate is denoted by V (x) and the total derivative of the Lyapunov candidate along the vector field by V˙ (x) = ∇V (x)f T (x). LaSalle’s invariance principle can be used for an autonomous (time-independent) or periodic system to prove asymptotic stability even if the total derivative of the Lyapunov candidate is only negative semi-definite: Definition 2 (LaSalle’s Invariance Principle [12]) Let Ω be a compact (closed and bounded) set with the property that every solution which starts in Ω remains for all future time in Ω. Let V : Ω → R be a continuously differentiable function such that V˙ (x) ≤ 0 in Ω. Let E be the set of all points in Ω where V˙ (x) = 0. Let M be the largest invariant set in E. Then every solution starting in Ω approaches M as t → ∞. Likewise to LaSalle’s invariance principle, Barbalat’s lemma can be used for nonautonomous (time-dependent) systems as it is the case in adaptive systems: Definition 3 (Barbalat’s lemma [19]) Assume f (t) is differentiable and has a limit, i.e. f → L and f˙ is uniformly continuous, then f˙ → 0. Using Lyapunov theory and Barbalat’s lemma one can guarantee stability and convergence for an adaptive control system under the influence of parametric uncertainty for arbitrary adaptation gains Γ and reference model dynamics. However, the system performance in model reference adaptive control depends to a great extent on the adaption gain Γ . The following characteristic system behaviour can be observed: • Small adaptation gain Γ : – Slow adaptation – Large tracking error for transient


22

• Large adaptation gain Γ : – Possible excitation of unmodeled dynamics – Oscillatory parameters

4.4.2

Non-parametric uncertainty

Non-parametric uncertainties such as

4

• measurement noise (e.g. from the gyroscope) • high frequency unmodeled dynamics (e.g. from actuator dynamics) • computation roundoff errors on the microcontroller and delays may lead to an unstable system even in case of proven Lyapunov-stability. An often observed issue in adaptive control is parameter-drifting and a resulting sudden divergence and instability known as bursting. This behaviour is mainly caused by a reference signal which contains not enough information, i.e. it is not persistently excited, combined with measurement noise. In the subsequent section modifications are proposed to ensure system robustness and are explained using the adaption law derived in 5.1: ˙ Jˆ¯ = −Γ F T ω ˜

(4.10)

Deadzone modification The deadzone modification is motivated by the observation that small tracking errors contain mainly noise and few information. Consequently, if the tracking error is small the adaptation is turned off. Otherwise the same adaptation law is used to adjust the controller. The modification of the adaptation law is then simply 5 0 if kek2 < ∆DZ ¯˙ = Jˆ −Γ F T ω ˜ otherwise where ∆DZ should be chosen according to the noise level in the measurements. To be concrete, ∆DZ should be above the noise level and below the signal level containing the adaptation information. If ∆DZ is chosen too high, that is above the signal level, then the adaptation law is in effect turned off. Due to its simplicity and effectiveness the deadzone modification is one of the most widely used modifications in adaptive control to ensure robustness. Parameter projection operator The parameter projection operator is motivated by the idea that often good bounds of the unknown parameter are a priori known. Using these predefined bounds the modification ensures that the integration "lies inside a bounded convex set" 6 containing the unknown parameter. Consequently, the parameters are bounded and unrealistic parameter values are avoided. A Simulink model of the projection operator for a ball can be seen in figure 4.3 and was implemented by Naira Hovakimyan [20]. The input to the projection operator is the center and radius of the ball as well as the signal to be integrated. Note that this modification has higher computational costs than the deadzone modification 4 cf.

[11]

5 Shown

is one possible implementation based on [13], page 73. Other implementations consider the tracking error element-wise or replace the two norm by the one norm. 6 cf. [13], page 71

23

4.4. Stability and robustness

Figure 4.3: Projection operator implementation in Simulink by Naira Hovakimyan [20]. The convex set is implemented as a ball with desired center (Prcenter), radius (Thetamax) and adaptation gain (EpsilonTheta).

in which essentially only one line of code needs to be added. To keep the computational costs low one could set bounds on the parameter without projecting them (cf. for example [4]). Of course bounding the parameters or using the projection operator always somehow limits the performance of the adaptive controller. For specific details concerning robustness properties and implementation of the projection operator please refer to [13] and [20]. σ-modification Besides the pure integration shown in the adaptation law 4.10 the σ-modification introduces a damping term 7 which becomes larger the more the ¯ deviates from zero. This is shown in the following equation parameter estimate Jˆ for a fixed σ-modification: ¯˙ = −Γ F T ω Jˆ ˜ + σΓ Jˆ¯

(4.11)

where σ is a tuning parameter. A small modification of the originally proposed σ-modification ensures that deviations from the nominal value Jˆ¯0 are considered instead: ¯˙ = −Γ F T ω Jˆ ˜ + σΓ (Jˆ¯ − Jˆ¯0 )

(4.12)

Thus the σ-modification acts like a "spring" and punishes large deviations from the nominal parameter value. e-modification The σ-modification defined in equation 4.11 has one disadvantage: If the tracking error becomes small the adaptation law is approximately equal to ˙ Jˆ¯ ≈ σΓ Jˆ¯

(4.13)

7 Often also called feedback or "leakage" term. This is why the σ-modification is sometimes referred to as leakage (cf. [13], page 65).


24

Thus the tendency of the σ-modification to return to the origin or the nominal value ¯0 leads to an "unlearning" 8 of the parameter values. Jˆ To avoid this downside, the e-modification defines the damping term in dependence of the tracking error: ¯˙ = −Γ F T ω Jˆ ˜ + σ kek2 Γ (Jˆ¯ − Jˆ¯0 )

8 cf.

[21], page 327

(4.14)

Chapter 5

Adaptive control of angular rates and online inertia estimation As described in section 4.3 the angular velocity equation needs to be linearly parametrized in order to estimate the inertia matrix J. A possible parametrization was developed in [17] where globally convergent tracking of angular velocity and inertia identification for a 3-DOF rigid body is shown. This parametrization was applied to quadrotors in [22] to adaptively control the attitude and estimate J online. However, in this project a modular controller is to be developed in which only the desired reference ωref and measurement from the gyroscopes ωmeas is available. In the next section, the parametrization based on [17] is derived and the stability and convergence properties are discussed using Lyapunov theory and Barbalat’s lemma introduced in subsection 4.4.1.

5.1

Derivation: Adaptation and control law

Based on the well known angular velocity equation of a rigid body ω˙ = J −1 (τ − ω × Jω)

(5.1)

a linear parametrization of the full inertia matrix J   Jxx Jxy Jxz J = Jyx Jyy Jyz  Jzx Jzy Jzz

(5.2)

can be found. Define the parameter vector J¯ to be estimated as J¯ = Jxx

Jyy

Jzz

Jyz

Jxz

Jxy

T

(5.3)

and the transformation T  p 0 T (ω) = 0 q 0 0 25

0 0 r 0 r 0 r q p

 q p 0

(5.4)

Chapter 5. Adaptive control of angular rates and online inertia estimation

With these definitions and the notation of  0 ω× =  r −q

the skew symmetric matrix ω ×  −r q 0 −p p 0

26

(5.5)

equation 5.1 can be written as ω˙ = J −1 (τ − ω × Jω) 0 = J −1 τ − J −1  r −q 

(5.6) −r 0 p

 q p 0 −p 0 q 0 0 0

  Jxx  Jyy   q   Jzz   p  Jxz   0  Jxz  Jxy

0 0 r 0 r 0 r q p

= J −1 τ − J −1 ω × T (ω)J¯

(5.7)

(5.8)

The angular velocity tracking error ω ˜ is given as ω ˜ = ω − ωdes

(5.9)

where ωdes is the desired angular velocity. The error equation for the angular velocity ω ˜˙ = ω˙ − ω˙ des

(5.10)

is now found by plugging in 5.6 in 5.10: ω ˜˙ = J −1 τ − J −1 · (˜ ω + ωdes )× J · (˜ ω + ωdes ) − ω˙ des

(5.11)

Using the identity J ω˙ des = T (ω˙ des )J¯  Jxx ⇔ Jyx Jzx

q˙des 0 p˙des

(5.12)   Jxx  Jyy   r˙des   Jzz     p˙des  Jyz    0  Jxz  Jxy (5.13)

ω ˜˙ = J −1 τ − J −1 · (˜ ω + ωdes )× T (˜ ω + ωdes )J¯ − ω˙ des

(5.14)

Jxy Jyy Jyz

 ˙  Jxz p p˙des Jyz  q  = 0 Jzz r des 0

0 q˙des 0

0 0

r˙des

0 r˙des q˙des

equation 5.11 can be written as =J

−1

τ −J

−1

· [(˜ ω + ωdes ) T (˜ ω + ωdes ) + J ω˙ des ]J¯ ×

= J τ − J · [(˜ ω + ωdes ) T (˜ ω + ωdes ) + T (ω˙ des )]J¯ = J −1 τ − J −1 F J¯ −1

−1

×

(5.15) (5.16) (5.17) (5.18)

where F is introduced as F = (˜ ω + ωdes )× T (˜ ω + ωdes ) + T (ω˙ des )

(5.19)

It can be proven via an appropriate Lyapunov function that the the adaptation and control law ˙ Jˆ¯ = −Γ F T ω ˜ (5.20) τ = −K ω ˜ + F Jˆ¯ (5.21)

27

5.2. Stability and convergence analysis Γ ∈ R3×3 : K ∈ R3×3 :

Adaptation gain of inertia matrix Controller gain acting on angular velocity error

˙ ensure that the angular velocity error ω ˜ and the parameter estimate error J˜¯ converge to zero [17]. However, this does neither imply that the parameter estimate Jˆ¯ ¯ actually converges to the true value of J. The detailed stabilconverges nor that Jˆ ity and convergence analysis using Lyapunov theory is discussed in section 5.2. The adaptation law in equation 5.20 is driven by the tracking error ω ˜ where the adaptation gain Γ defines the speed of the adaptation. It is important to note that the adaptation gain Γ , F and the tracking error ω ˜ are independent of any other parameters. Consequently, the inertia can be estimated without having to know any other system parameters. This is a big advantage compared to other estimation schemes such as recursive least squares 1 . In addition, a general advantage of adaptive control in contrast to robust control is that it is not necessary to define bounds on the inertia estimate. The control law given in equation 5.21 is composed of two parts: The first part is driven by the tracking error ω ˜ where the controller gain K defines the aggressiveness of the controller. The second part of the control law incorporates the inertia estimate and ensures a long-time learning behaviour of the controller: The longer the flight the more accurate is the inertia estimate and the better the controller performance. Parametrization modification For the implementation on a quadrotor one can assume that the "body axes are close to the principal axes" 2 such that the offdiagonal terms in the inertia matrix can be neglected     Jxx Jxy Jxz Jxx 0 0 Jyy 0  J = Jyx Jyy Jyz  ≈  0 (5.22) Jxy Jyz Jzz 0 0 Jzz This yields the following simplification for J¯ J¯ = Jxx Jyy

Jzz

T

(5.23)

and the transformation matrix T (ω)  p 0 T (ω) = 0 q 0 0

5.2

 0 0 r

(5.24)

Stability and convergence analysis

Asymptotic tracking First note that plugging in the control law into the error equation 5.18 yields

1 cf. 2 [7]

table 5.1

˜˙ = J −1 τ − J −1 F J¯ ω ˆ¯ − J −1 F J¯ = J −1 (−K ω ˜ + F J)

(5.25)

¯ = −J −1 K ω ˜ + J −1 F (Jˆ¯ − J) = −J −1 K ω ˜ + J −1 F J˜¯

(5.27)

(5.26) (5.28)


28

˜ ¯ Right: 3-D plot for total derivative Figure 5.1: Left: 3-D plot for Lyapunov candidate V (˜ ω , J). ˜ ˜ 3 ¯ Remark: ω −V˙ (˜ ω , J). ˜ and J¯ are actually vectors in R . Only one coordinate was considered for visualization purposes

and recall the adaptation law ˙ ˙ J˜¯ = Jˆ¯ − J¯˙ = −Γ F T ω ˜

(5.29)

It can easily be verified that x∗ = (0, 0) is an equilibrium of equations 5.28 and 5.29. To show asymptotic tracking a standard Lyapunov candidate was proposed in [17]: ˜¯ = 1 (˜ ˜¯ V (˜ ω , J) ωT J ω ˜ + J˜¯T Γ −1 J) 2

(5.30)

˜¯ along the vector field is given The total derivate of the Lyapunov candidate V (˜ ω , J) by ˜ = ∇V (˜ ˜¯ ¯ ¯˜ T (˜ V˙ (˜ ω , J) ω , J)f ω , J) " # h i ω ˜˙ ∂V = ∂V ˙ ˜ ∂ω ˜ ∂ J¯ J¯˜ h i −1 ˜¯ −1 −J K ω ˜ + J F J ˜ T T −1 = ω ˜ J J¯ Γ −Γ F T ω ˜ ˜ T −1 −1 ¯ − J˜¯T Γ −1 Γ F T ω =ω ˜ J · (−J K ω ˜ + J F J) ˜

(5.31) (5.32) (5.33) (5.34)

= −˜ ωT K ω ˜ +ω ˜ T F J˜¯ − J˜¯T F T ω ˜

(5.35)

= −˜ ω Kω ˜

(5.36)

T

˜ ˜¯ 3 is semi-positive definite, case (a) of ¯ is positive definite and −V˙ (˜ Since V (˜ ω , J) ω , J) section 4.4.1 applies: The equilibrium x∗ is stable in the sense of Lyapunov. Using Barbalat’s Lemma and Theorem 8.4 from [12] it follows that the tracking error ω ˜ ˜ ¯ asymptotically converges to zero for any initial value of ω ˜ (0) and J(0), i.e. ω ˜ → 0. Parameter estimation convergence In this section only the relevant results are stated and the consequences for the application on a quadrotor is discussed. Please refer to [17] for proofs. 3 The

controller gain matrix K is chosen to be positive definite.

29

5.2. Stability and convergence analysis

In [17] it is shown that the inertia estimate Jˆ¯ converges to the true parameter vector J¯ under the following conditions: First define × W (ωdes , ω˙ des ) = ωdes T (ωdes ) + T (ω˙ des )

(5.37)

  W (t1 ) .  ¯ (t1 , ..., t6 ) =  W  .. 

(5.38)

and

W (t6 )

Suppose now there exist a δ > 0 and a γ > 0 such that for every T ≥ 0 there exist ¯ (t1 , ..., t6 )) > γ. Then lim Jˆ¯ = J. ¯ t1 , ..., t6 ∈ [T, T + δ] such that σmin (W t→∞

Upper bound on torque In this section a conservative upper bound on the torque τ required to track reference signal ωdes is given 4 . This bound is useful in determining if the quadrotor’s actuators are able to track the desired reference or if the rotor angular speed may run into saturation. Define the bounds m1 ≥ 0, m2 ≥ 0, η1 > 0, η2 > 0 and mJ¯ as follows k˜ ω (0)k2 ≤ m1

(5.39)

˜¯

J(0) ≤ m2

(5.40)

kωdes k2 ≤ η1

(5.41)

kω˙ des k2 ≤ η2

(5.42)

2

J¯ = Jxx 2

Jyy

Jzz

Jyz

Jxz

Jxy 2 ≤ mJ¯

σJsup ≥ σmax (J) ≥ σmin (J) ≥ σJinf

M = σmax (K)m ¯1 +

√

6(m ¯ 1 + η1 )2 (m ¯ 2 + mJ¯) +

√

6(m ¯ 2 + mJ¯)η2

(5.43) (5.44)

(5.45)

where s m ¯1 =

σJsup σJinf

+

m22 σmin (Q)σJinf

s ¯2 = m 4 The

proof is given in [17]

σJsup σmax (Q)m21 +

σmax (Q) 2 m σmin (Q) 2

(5.46)

(5.47)


30

Then ∀t ≥ 0 k˜ ω (t)k2 ≤ m ¯1

(5.48)

˜¯ ¯2

J(t) ≤ m

(5.49)

kτ (t)k2 ≤ M

(5.50)

2

and

Parametrization modification Following the proof in [17] and assuming a diagional inertia matrix one can show that the conservative upper bound on the torque simplifies to: M = σmax (K)m ¯ 1 + ((m ¯ 1 + η1 )2 + η2 )(m ¯ 2 + mJ¯)

5.3

(5.51)

Simulation results

In this section the derived adaptation law and control law is analyzed with respect to asymptotic tracking performance and parameter estimation convergence. Three different cases are presented in order to see the effects of the actuator dynamics and measurement noise of the gyroscopes.

5.3.1

Case I: Parametric uncertainty

In the first case the model of the plant consists only of the angular velocity update equation ω˙ = J −1 (τ − ω × Jω)

(5.52)

The block diagram of the controller and plant is shown in figure 5.2:

ωdes

Reference model

ωref

Tdes ω ˜

Control law τ = −K ω ˜ + F Jˆ¯

τ

Control allocation

ndes

Plant ω˙ = J

−1

ω˙

R

ω

−

˜ ω

(τ − ω × Jω)

Adaptation law ˙ Jˆ¯ = −ΓF T ω ˜

Figure 5.2: Case I: Parametric uncertainty. Actuator dynamics and measurement noise are not included

The actuator dynamics and measurement noise are not included. The desired angular velocity ωdes as depicted in figure 5.3 is the same for all three cases: Every axis is excited for a short period of time to calibrate the inertia parameter estimate.

31

5.3. Simulation results

Desired angular velocity ωdes ωdes [rad/s]

1

pdes qdes rdes

0.5 0 −0.5 −1

0

10

20

30

40

50

60

70

Time t [s] Tracking error ω ˜

·10−3

p˜ q˜ r˜

ω ˜ [rad/s]

2 0 −2

0

10

20

30

40

50

60

70

Time t [s] Inertia estimate Jˆ

ˆ J [kg m2 ] J,

·10−3

Jˆxx Jˆyy Jˆzz J

8 6 4 2 0

0

10

20

30

40

50

60

70

Time t [s]

Figure 5.3: Simulation results case I: Parametric uncertainty with low adaptation gain: K = 1 · 13×3 , Γ = 1 · 13×3

In figure 5.3 the controller gain K acting on the tracking error is set to 1 · 13×3 5 and the adaptation gain Γ is set to 1 · 13×3 . The tracking error ω ˜ converges to zero after about five seconds. At the same time the inertia parameter estimate starts to T T converge to the true value Jxx Jyy Jzz = 5.6 · 10−3 5.6 · 10−3 8.9 · 10−3 without drifting when not excited. To validate the performance of the control and adaptation law the adaptation gain Γ is set to 10 · 13×3 as shown in figure 5.4. The parameter estimate jumps instantaneously to the correct parameter value and also the tracking error converges to zero in less than a second. However, as described in chapter 4 too high adaptation gains may lead to oscillations and possibly instability of the control system when non-parametric uncertainty is included. It is important to note that it is necessary to have information in the desired angular velocity ωdes . Otherwise, the parameter estimate cannot adapt. In figure 5.3 and 5.4 it can be seen that the initial tracking error is differently large for around the x,y and z axis. This is firstly due to the fact that the inertia around 51 3×3

denotes the 3 × 3 identity matrix


32

the z axis is higher than around the x and y axis. Secondly, in this case the gain of the reference model for the y axis is set to a larger value than for the x axis. This is why the initial error around the y axis is slightly bigger than around the x axis. Desired angular velocity ωdes ωdes [rad/s]

1

pdes qdes rdes

0.5 0 −0.5 −1

0

10

20

30

40

50

60

70

Time t [s]

ω ˜ [rad/s]

1

Tracking error ω ˜

·10−3

p˜ q˜ r˜

0.5 0 0

10

20

30

40

50

60

70

Time t [s] Inertia estimate Jˆ

ˆ J [kg m2 ] J,

·10−3

Jˆxx Jˆyy Jˆzz J

8 6 4 2 0

0

10

20

30

40

50

60

70

Time t [s]

Figure 5.4: Simulation results case I: Parametric uncertainty with high adaptation gain to validate the performance of the control and adaptation law: K = 1 · 13×3 , Γ = 10 · 13×3

5.3.2

Case II: Parametric uncertainty and actuator dynamics

In the second case the actuator dynamics of the rotors are included as depicted in the block diagram 5.5. The measurement noise is neglected. In figure 5.6 K is set to 1· 13×3 and Γ is set to 1· 13×3 . The results can be compared with figure 5.2: Note that the initial tracking error is higher than in figure 5.2 due to the delay in the rotor response. The tracking error decreases slower than in the first case. While the actuator dynamics have apparently negative effects on the tracking error the parameter estimate is not influenced visibly: The inertia estimates converge to the true values and do not drift. To cope with the influence of the actuator it is possible to introduce a so called pseudo-hedge control [4]. The pseudo-hedge control signal is the difference between the desired angular acceleration ω˙ des and the predicted angular acceleration ω ˆ com-

33


ωdes

Reference model

ωref

Tdes ω ˜


τ

Control allocation

ndes

ω˙ Actuator n ω˙ = J −1Plant (τ − ω × Jω)

R

ω

−

˜ ω


Figure 5.5: Case II: Parametric uncertainty and actuator dynamics. Measurement noise is not included


1

pdes qdes rdes

0.5 0 −0.5 −1

0

10

20

30

40

50

60

70

Time t [s]

ω ˜ [rad/s]

Tracking error ω ˜ p˜ q˜ r˜

0.2 0 −0.2

0

10

20

30

40

50

60

70

Time t [s]

ˆ J [kg m2 ] J,

1

Inertia estimate Jˆ

·10−2

Jˆxx Jˆyy Jˆzz J

0.5 0 0

10

20

30

40

50

60

70

Time t [s]

Figure 5.6: Simulation results case II: Parametric uncertainty and actuator dynamics: K = 1 · 13×3 , Γ = 1 · 13×3

ing from the plant. The prediction is made based on a model of the actuator and the plant. The idea of pseudo-hedge control is then to slow down the dynamics of the reference model such that the quadrocopter’s actuators can follow the reference signal. The only caveat of this approach is the computational cost arising from the actuator and plant model.


5.3.3

34

Case III: Parametric uncertainty, actuator dynamics and measurement noise

In the third case, the influence of parametric uncertainty, unmodeled actuator dynamics and measurement noise coming from the gyroscopes is analyzed as shown in the block diagram in figure 5.7. The simulation results are shown in figure 5.8. ωdes

Reference model

ωref noise

Tdes ω ˜


τ

Control allocation

ndes


R

+

ω

ωm

−

˜ ω

+


Figure 5.7: Case III: Parametric uncertainty, actuator dynamics and measurement noise

The tracking error in this simulation is noisy due to the added noise on the gyroscopes. At first sight the inertia estimate looks similar to the simulations above, however, note that the inertia estimate starts to drift slightly in the end when the individual axis are not excited anymore. Parameter drift occurs due to nonparametric uncertainty and poorly excited reference signals and is a often observed problem in adaptive control. While the parameter drift may not look very dangerous in this case, it may eventually lead to bursting which is a common phenomena in adaptive control and analyzed in detail in the next subsection. Fortunately, there are several methods to make the controller more robust.

35



1

pdes qdes rdes

0.5 0 −0.5 −1

0

10

20

30

40

50

60

70

Time t [s]

ω ˜ [rad/s]

Tracking error ω ˜ p˜ q˜ r˜

0.5 0 −0.5 0

10

20

30

40

50

60

70

Time t [s]

ˆ J [kg m2 ] J,

1


·10−2

Jˆxx Jˆyy Jˆzz J

0.5 0 0

10

20

30

40

50

60

70

Time t [s]

Figure 5.8: Simulation results case III: Parametric uncertainty, actuator dynamics and measurement noise: K = 1 · 13×3 , Γ = 1 · 13×3


5.3.4

36

Bursting and deadzone modification

A common issue in adaptive control is a phenomena called bursting. If the information content of the signal is small i.e. the system is not persistently excited then the adaptive controller is not able to identify the parameters. The parameter starts to drift in a direction where the tracking error ω ˜ is kept small. This drifting is continued until the set of estimated parameters lead to a system which is unstable. Consequently, the parameter estimates and tracking error diverge. In figure 5.9 quantization errors are modeled and only the yaw axis is excited. The parameters drift and after around 600 s finally bursting occurs.

ωdes [rad/s]

Desired angular velocity ωdes pdes qdes rdes

1 0 −1 −2

0

1

2

3

4

5

6

7

8

9

Time t [s]

ω ˜ [rad/s]

Tracking error ω ˜ 80 60 40 20 0 −20

p˜ q˜ r˜

0

100

200

300

400

500

600

Time t [s]

Jˆ [kg m2 ]

4


·10−2

Jˆxx Jˆyy Jˆzz

2 0 −2

0

100

200

300

400

500

600

Time t [s]

Figure 5.9: Bursting occurs, K = 0.1 · 13×3 , Γ = 0.1 · 13×3

The deadzone modification introduced in subsection 4.4.2 is an often used modification of the adaptation law to ensure controller robustness. It can be defined as ˙ 0 if kek2 < ∆DZ ˆ ¯ J= −Γ F T ω ˜ otherwise As mentioned in subsection 4.4.2 the deadzone modification is motivated by the assumption that small tracking errors contain mainly noise and disturbances and should not be used to adapt the parameter. Consequently, the adaptation is only

37


turned on iff the tracking error is outside of the deadzone area. To be more concrete, consider 5.10 The same desired angular velocity ωdes and parameter set as in figure 5.9 was used. The only difference is that the adaptation law is modified by a deadzone. First, the tracking error around the z axis is above the deadzone ∆DZ and thus the adaptation is turned on. After some second the inertia estimate is good enough such that the tracking error falls below the deadzone threshold and the adaptation is turned off. As a result, the inertia estimate around z axis does not drift anymore and converges to approximately the true value. Furthermore, the inertia estimates around the x and y axis remain zero since the information content is too low for adaptation.

ωdes [rad/s]


1 0 −1 −2

0

1

2

3

4

5

6

7

8

9

Time t [s] Tracking error ω ˜ p˜ q˜ r˜ ∆DZ

ω ˜ [rad/s]

0.2 0 −0.2 −0.4

0

100

200

300

400

500

600

ˆ J [kg m2 ] J,

Time t [s] 1 0.8 0.6 0.4 0.2 0


·10−2

Jˆxx Jˆyy Jˆzz J 0

100

200

300

400

500

600

Time t [s]

Figure 5.10: Dead-zone modification, K = 0.1 · 13×3 , Γ = 0.1 · 13×3


5.3.5

38

Composite adaptation

As described in subsection 4.2.3 composite adaptation uses the tracking error e and the prediction error ep obtained e.g. by RLS to improve transient performance of the adaptive controller. This approach is described for example in [16] and [11].

Recursive Least Squares estimation The equations to estimate the diagonal elements of the inertia matrix are listed in table 5.1 Equation

∆

θ

Φ

Jxx p˙ ≈ kn l(−n22 + n24 )

p˙

1 Jxx

kn l(−n22 + n24 )

Jyy q˙ ≈ kn l(n21 − n23 )

q˙

1 Jyy

kn l(n21 − n23 )

Jzz r˙ ≈ kn km (−n21 + n22 − n23 + n24 )

r˙

1 Jzz

kn km (−n21 + n22 − n23 + n24 )

Table 5.1: Inertia estimation using least squares estimation

Recall the equations for recursive least squares K = P (k − 1)Φ(k − 1)(R + Φ(k − 1)T P (k − 1)Φ(k − 1))−1 ˆ ˆ − 1) + K[∆(k) − ΦT (k)θ(k)] ˆ θ(k) = θ(k

(5.53)

P (k) = (I − KΦ(k))P (k − 1)

(5.55)

(5.54)

Note that in contrast to the gradient descent parameter estimation described in section 5.1, in RLS the parameters kn , km and l need to be known. If gradient descent and recursive least squares estimation is used in parallel it is possible to calculate kn , km and l by comparing the estimates.

ωdes

Reference model

ωref noise

Tdes ω ˜


τ

Control allocation

Adaptation law

˙ Jˆ¯ = −ΓRLS (Jˆ¯ − θ) − ΓF T ω ˜

ndes


ˆ˙ θ Recursive Least Squares ω ˜ ω

R

+

ω

ωm

−

˜ ω

+

Estimation of ω˙

Figure 5.11: Composite adaptation

Estimation of ∆ A forward backward kalman filter (which is also known as fixed T point smoother6 ) is used to estimate ∆ = p˙ q˙ r˙ since no direct measurement of the angular body acceleration is available. Note that simply taking the derivative would amplify the noise! 6 [16]

39


The equations of the forward backward kalman filter are x ˆ˙ (1) = x ˆ(4) x ˆ˙ (2) = x ˆ(5)

(5.56)

x ˆ˙ (3) = x ˆ(6) ˙x ˆ(4) = 0

(5.58)

x ˆ˙ (5) = 0 x ˆ˙ (6) = 0

(5.60)

(5.57) (5.59) (5.61) (5.62)

x ˆp = x ˆ+x ˆ˙ · dt

 0 0  0 A= 0  0 0

0 0 0 0 0 0

0 0 0 0 0 0

1 0 0 0 0 0

0 1 0 0 0 0

(5.63)

 0 0  1  0  0 0

(5.64)

L = I(6);

(5.65)

P˙p =AP + P AT + LQc LT Pp = P + P˙p · dt

(5.66)

 1 H = 0 0

0 1 0

0 0 1

0 0 0

0 0 0

 0 0 0

K = Pp H T (HPp H T + Mc RMcT )−1

(5.67)

(5.68)

(5.69)

x ˆm = x ˆp + K(z − x ˆp )

(5.70)

Pm = (I(6) − KH)Pp

(5.71)

And finally xN = xN + x ˆm − x ˆp

(5.72)

Composite adaptation law In composite adaptation, the adaptation law is composed of the gradient descent estimate and the RLS estimate, weighted appropriately. With reference to [16], page 94 the adaptation law given in equation 5.20 is exchanged by the following new adaptation law: ¯˙ = ΓRLS (Jˆ¯ − θ) − Γ F T ω Jˆ ˜

(5.73)


40

The control law in 5.21 remains unchanged: τ = −K ω ˜ + F Jˆ¯

(5.74)

The improved transient response of the composite adapation controller can be seen in figure 5.12 where the tracking error of the traditional model reference adaptive controller and the composite adaptation is compared. Note that the model is simulated using the exact same parameters and system inputs for both approaches.

Model reference adaptive control: Tracking error ω ˜ p˜ q˜ r˜

ω ˜ [rad/s]

0.2 0 −0.2 −0.4

0

10

20

30

40

50

60

70

Time t [s] Composite adaptation: Tracking error ω ˜ p˜ q˜ r˜

ω ˜ [rad/s]

0.1 0 −0.1 −0.2

0

10

20

30

40

50

60

70

Time t [s]

Figure 5.12: Simulation results: Transient response of model reference adaptive control and composite adaptation

5.4

Implementation results

Due to the high computational costs of the composite adaptation approach only the traditional model reference adaptive controller is implemented on the quadrocopter. The test setup is shown in figure 5.13. The adaptive controller is running onboard and sends the parameters of the inertia to the ground station in real-time. The data is sent using a serial interface or XBee transmitter. To ensure that the controller only adapts parameters while in air one small modification was made: Only when the thrust is above a certain threshold the adaptation law is turned on, otherwise the adaptation is turned off 7 as it can be seen in figure 5.14: First the power of the quadrocopter is turned on and the initial estimate of the inertia is used in the adaptation law. After take off, the inertia starts to adapt due to the tracking error ω ˜ and converges after around 40 s. After landing 7 That

˙ is Jˆ is zero

41

5.4. Implementation results

the quadrocopter the adaptation law is automatically off and the inertia estimate remains at it’s last value.

Figure 5.13: Implementation setup: The adaptive controller is running onboard and sends the inertia estimate in real-time to the ground station

The bottom figure in 5.14 shows the desired angular velocity pdes , the angular velocity computed by the reference model pref and the measured angular velocity p. Note that only the angular velocity around the x axis for the time interval 112 s to 130 s is shown. To deal with the slight overshoot in the angular velocity one could introduce the pseudo-hedge control described in subsection 5.3.3. However, the computational cost of modeling the actuator and plant onboard of the quadrocopter was not feasible due to CPU limitations. Inertia estimate Jˆ

ˆ J [kg m2 ] J,

·10−2

Jˆxx Jˆyy Jˆzz

1.5 1 0.5 0

0

20

40

60

80

100

120

140

160

Time t [s] pdes , pref , p [rad/s]

Angular velocity: Desired pdes , reference model pref , measured p pdes pref p

2 0 −2 112

114

116

118

120

122

124

126

128

130

Time t [s] Figure 5.14: Implementation results: In the top figure the inertia estimate around the x,y and z axis is illustrated. Note that the adaptation is automatically turned on when the quadrocopter is in air. The bottom figure shows the desired angular velocity, the angular velocity computed by the reference model and the measured angular velocity - each around the x axis


42

Chapter 6

Thrust control and online mass estimation In this chapter a feedforward control of the thrust based on the mass estimation is presented. The thrust reference Tdes as depicted in figure 6.1 is usually commanded by the pilot. The control allocation uses this reference signal and the estimated mass m ˆ to compute the desired motor speed ndes . ωdes

ωref

Reference model

noise

Tdes ˜ ω


ndes

Control allocation

τ m ˆ


R

+

ω

ωm

−

˜ ω

+

Recursive Least Squares


Figure 6.1: Feedforward control of thrust

In the next section, the batch least square and recursive least square algorithm to estimate the mass m is presented.

6.1

Batch least squares

The mass of a quadrotor can be estimated via1 m¨ z = cos(φ) cos(θ)T − mg T = kn

4 X

n2i

(6.1) (6.2)

i=1

Note that the rotation matrices defined in appendix A.2 were used. The estimate of the mass is then n

m ˆ =

1 X cos(φ) cos(θ)T (i) gn i=1 z¨(i)

1 [7]

43

(6.3)

Chapter 6. Thrust control and online mass estimation

44

where n is the number of measurements. During hover (¨ z ≈ 0) the above equations simplify to 0 = cos(φ) cos(θ)T − mg T = kn

4 X

(6.4)

n2i

(6.5)

1 X m ˆ = cos(φ) cos(θ)T (i) gn i=1

(6.6)

i=1

and n

6.2

Recursive least squares

To recursively estimate the mass m, equation 6.1 can be rewritten to 1 cos(φ) cos(θ)T m

z¨ + g =

(6.7)

which now has the desired form described in section 4.3: ∆ = θT Φ

(6.8)

with ∆ = z¨ + g, the regressor Φ = cos(φ) cos(θ)T and the parameter estimate 1 θT = θ = m . Note that we are estimating the inverse of the mass m. During hover, equation 6.7 simplifies to g=

1 cos(φ) cos(θ)T m

(6.9)

1 and y = g, Φ = cos(φ) cos(θ)T and θ = m . The equations for the recursive least squares are (cf. [16]):

K = P (k − 1)Φ(k − 1)(R + Φ(k − 1)T P (k − 1)Φ(k − 1))−1 ˆ ˆ − 1) + K[∆(k) − ΦT (k)θ(k ˆ − 1)] θ(k) = θ(k

(6.10)

P (k) = (I − KΦ(k))P (k − 1)

(6.12)

Conditions

General

Hover

(6.11)

Equation

∆

θ

Φ

m¨ x = − sin(φ)T + Fx ≈ Fx

x ¨

Fx m

1

m¨ y = sin(φ) cos(θ)T + Fy ≈ Fy

y¨

Fy m

1

m¨ z = cos(φ) cos(θ)u1 − mg

z¨ + g

1 m

cos(φ) cos(θ)T

0 = cos(φ) cos(θ)T − mg

g

1 m

cos(φ) cos(θ)T

Table 6.1: Least squares estimation [7]: General assumptions: sin(φ) = sin(θ) ≈ 0, Hover assumptions: z¨ ≈ 0

45


6.3

Simulation results

Case I: Assuming perfect knowledge of kn In a first simulation, perfect N knowledge of kn = 5.1978 · 10−6 RPM 2 was assumed. The simulation model includes actuator dynamics and measurement noise of the gyroscopes. The initial parameter estimate θˆ0 is set to 1 kg and the initial variance P0 is set to 1. In figure 6.2 one can see that the parameter estimate θˆ converges instantly to the correct value and the variance P asymptotically converges to zero (cf. figure 6.2, blue line).

ωdes [rad/s]


0.1 0 −0.1 −0.2

0

10

20

30

40

50

60

70

80

90

100

Time t [s]

n [RPM]

Motor speeds n n1 n2 n3 n4

400 200 0

10

20

30

40

50

60

70

80

90

100

Time t [s] Mass estimate m ˆ

m ˆ [kg]

1

m, ˆ kn ≈ 5.2 · 10−6 m, ˆ kn ≈ 5.2 · 10−12 m

0.8 0.6 0.4 0

10

20

30

40

50

60

70

80

90

100

P

Time t [s] 1 0.8 0.6 0.4 0.2 0

Variance P in mass estimate

·10−6

P , kn ≈ 5.2 · 10−6 P , kn ≈ 5.2 · 10−12

0

10

20

30

40

50

60

70

80

90

100

Time t [s]

Figure 6.2: Simulation results: Recursive least squares estimation of mass, hover equations, random input, kn = 5.1978 · 10−6 N/RPM2 assumed to be known. Parameters: measurement noise 1.0 · 10−3 , R = 1.0 · 10−3 , P0 = 1, θˆ0 = 1.0 kg, m = 0.298 kg

Case II: Uncertainty in parameter kn In a second simulation, a wrong parameter value for kn is used. From figure 6.2 it becomes obvious that at least the order of magnitude of kn should be known to estimate the mass m correctly. Otherwise the least square algorithm cannot fit the input/output data correctly and the estimate drifts away (cf. figure 6.2, green line).


46

In [4] the parameter kn is averaged for a flexible propeller using test-bench data (cf. figure 6.3). In this project, however, different propellers were used such that kn is calculated in a test hover flight (cf. section 6.4).

Figure 6.3: Averaged values for kn using AscTec test-bench data [4]

6.4

Implementation results

In a first step the correct value of kn was calculated using data from a test hover flight and the quadrocopter mass measured by a weighing scale. In particular, the rotor angular speeds measured during hover, as depicted e.g. in figure 6.4, were averaged. Using the following equation and m = 0.294 kg, φ = θ ≈ 0 kn =

mg (n21 + n22 + n23 + n24 )

kn was approximated by kn ≈ 9.2038 · 10−8 calculated in [4]).

N RPM2

(cf. with kn = 5.68 · 10−8

(6.13) N RPM2

Hover flight The mass is estimated based on data from a second flight using the recursive least squares equations from section 6.2. The initial parameter estimate θˆ0 is set to 1 kg and the initial variance P0 is set to 1. In figure 6.4 one can see that the mass parameter estimate converges after about 2 s to the final value of 0.295 kg which corresponds approximately to the measured mass on the weighing scale. The result obtained by the recursive least squares algorithm can be validated by reading off the average value for u1 from figure 6.4 which is around u1,avg ≈ 2.9 kg·m s2 : m=

2.9 kg·m u1 s2 ≈ = 0.2956 kg g 9.81 sm2

(6.14)

47

6.4. Implementation results

Measured rotor speed n

n [RPM]

90

n1 n2 n3 n4

80 70 60 50

0

2

4

6

8

10

12

14

Time t [s] Attitude φ, θ, ψ φ, θ, ψ [rad]

2

φ θ ψ

1 0 0

2

4

6

8

10

12

14

10

12

14

Time t [s] Thrust T T [kg m / s2 ]

2.94 2.92 2.9 2.88 2.86 0

2

4

6

8

Time t [s]

m, ˆ m [kg]

Mass estimate m ˆ 0.34 0.32 0.3 0.28 0.26

m ˆ m

0

2

4

6

8

10

12

P

Time t [s] 1 0.8 0.6 0.4 0.2 0

Variance P in mass estimate

·10−5

0

2

4

6

8

10

12

14

Time t [s]

Figure 6.4: Implementation results:Mass parameter estimation during hover: θˆ0 = 1 kg, P0 = 1, θˆf inal ≈ 0.295 kg, mmeas ≈ 0.295 kg


48

Chapter 7

Conclusion and outlook This chapter is structured into two parts: In section 7.1 the theoretic, simulation and implementation results of the adaptive controller and inertia estimation are discussed and the advantages and disadvantages compared to other approaches are highlighted. Section 7.1 concludes with possible future works. In section 7.2 the mass estimation and thrust control is reviewed and possible extensions are proposed. For a quick overview the possible future works of both parts are shown in table 7.1 .

7.1 7.1.1

Adaptive control of angular body rates and inertia estimation Discussion

In the first part of this semester project an adaptive controller was developed that controls the angular body rate and learns the inertia during flight. The adaptive controller runs onboard and sends the inertia estimate in real-time to the ground station for validation purposes. The linear parametrization which is based on [17] is a very elegant way to simultaneously control the body angular rates and estimate the inertia matrix in a CPUefficient way. Note that the adaptation gain Γ , the matrix F = (˜ ω + ωdes )× T (˜ ω+ ˜ of the adaptation law ωdes ) + T (ω˙ des ) and the tracking error ω J¯˙ = −Γ F T w ˜

(7.1)

are either constant or functions of the desired angular velocity ωdes and measurement ωmeas . Consequently, both the control and the adaptation law do not depend on any system parameters. That is, in constrast to other estimation architectures this approach does not need to know any parameters in advance. Compare this approach with the measurement equation used by a Kalman filter, recursive least square or any other estimation scheme using the prediction error ep : Jxx p˙ ≈ kn l(−n22 + n24 ) Jyy q˙ ≈ Jzz r˙ ≈

kn km (−n21 + kn l(n21 − n23 )

n22

(7.2) −

n23

+

n24 )

(7.3) (7.4)

Note that in order to estimate the diagonal elements of the inertia matrix J the thrust constant kn , motor constant km and the arm length l of the quadrocopter 49

Chapter 7. Conclusion and outlook

50

Adaptive control of angular body rates and inertia estimation

Mass estimation and thrust control

• Extension of adaptive controller to other platforms (hexacopter/quadrocopter, aerial robotic manipulator, cameras, other hardware changes)

• Introducing a forgetting factor for recursive least squares to deal with changing parameters

• Comparison of adaptive controller to other controllers and performance benchmarking

• Estimation of payload mass and inertia • Considering payload inertia explicitely in control law • Replacing RLS by other estimation schemes and benchmarking

Table 7.1: Overview of possible future works

need to be known. In addition, the derivative of the angular velocity cannot be measured directly and needs to be estimated using for example a Kalman filter which leads to higher, for our system infeasible, computational costs. The measurements of the angular velocity ω provided by the gyroscopes used in this project are highly corrupted by noise and thus estimating the derivative of the angular velocity is prone to introducing errors. Recall that the goal of this project is to design a controller that can be used on different platforms with gyroscopes of different quality. Consequently, one would need to retune the measurement noise R of the Kalman filter for each platform. In contrast, the adaptation and control law of the implemented algorithm do not depend on the angular acceleration ω. ˙ Furthermore, as shown in section 5, using the implemented adaptation law it is in fact possible to estimate the full inertia matrix J, that is, not only the diagonal but also the off-diagonal elements of the inertia matrix. A big advantage of adaptive control compared with, for example robust control, is that no uncertainty bounds need to be defined. This means that in principle no prior knowledge of the inertia is needed. If no good inertia estimate is available it should be set to zero. Of course, the better the prior estimate, the faster the inertia will converge to the true value, depending also of course on the chosen adaptation gain Γ .

7.1.2

Future works

The implemented controller can now be tested on a variety of different platform equipment, such as UAVs with aerial robotic manipulator (cf. figure 7.1), cameras, or heavy payload. For the use of the controller on a different multirotor configuration, such as on a hexacopter, only the control allocation matrix needs to be exchanged. In order to analyze the performance, the adaptive controller can now be compared to other control approaches on a precomputed trajectory. This way, the stability, transient behavior and long-time learning can be benchmarked with respect to a baseline controller 1 . 1 For

instance, a simple P-controller which is available on the original AscTec Simulink SDK

51

7.2. Mass estimation and thrust control

Figure 7.1: Current research topic at ASL: Quadrocopter equipped with aerial robotic manipulator 2

7.2 7.2.1

Mass estimation and thrust control Discussion

In the second part it was shown that the mass m can be estimated robustly. For the special case of hovering, the point mass equation simplifies to g=

4 X 1 kn n2 m i=1

(7.5)

We can assume that the gravitational acceleration g is known and constant. In addition, for thrust control we only need the ratio of kmn . Thus, measuring the rotor speeds is enough to estimate the mass during hover. Either rotor speed measurements are available or simply the commanded rotor speeds are used.

7.2.2

Future works

Mellinger et al. [7] showed that it is possible to estimate the weight of the payload 3 using recursive least squares by introducing a forgetting factor. The forgetting factor basically ensures that the squared errors are minimized with respect to more recent data. This way the RLS can deal with changing parameters, such as different payload (cf. figure 7.3). Besides introducing a forgetting factor, it would also be interesting to replace the recursive least squares algorithm by a different estimation scheme such as a Kalman Filter and to compare the performance and computational cost. Knowing the inertia of the payload would be important to control a quadrocopter platform and a gripper on which a research project is focused at the Autonomous Systems Lab.

2 Photograph taken from a proposed Semester Project 2013, at Autonomous Systems Lab (ASL), ETH Zurich. The original photograph is from the Grasp Laboratory, University of Pennsylvania 3 The quadrocopter weight is assumed to be known in [7]

Chapter 7. Conclusion and outlook

52

Figure 7.2: Quadrocopter equipped with gripper to pick up and drop payload of different mass distribution and different inertia [7]

Figure 7.3: The weight of the payload is estimated using recursive least squares with forgetting factor to deal with payload of different weight. In addition, Mellinger et. al showed that it is possible to estimate the payload inertia [7]

Appendix A

Math A.1

Euler angles and angular velocity

Purely geometric mapping from euler angles (ϕ, θ, ψ) (i.e. tait-bryan angles) to T body angular speed ω = p q r :    p 1 q  = 0 r 0

A.2

0 cos(ϕ) − sin(ϕ)

  ϕ˙ − sin(θ) sin(ϕ) cos(θ)   θ˙  cos(ϕ) cos(θ) ψ˙

(A.1)

Rotation matrices

The rotation matrix from inertial to body frame is given by   c(ψ)c(θ) c(θ)s(ψ) −s(θ) RIB (φ, θ, ψ) = c(ψ)s(φ)s(θ) − c(φ)s(ψ) c(φ)c(ψ) + s(φ)s(ψ)s(θ) c(θ)s(φ) s(φ)s(ψ) + c(φ)c(ψ)s(θ) c(φ)s(ψ)s(θ) − c(ψ)s(φ) c(φ)c(θ) (A.2) The rotation matrix from body to inertial frame is given by   c(ψ)c(θ) c(ψ)s(φ)s(θ) − c(φ)s(ψ) s(φ)s(ψ) + c(φ)c(ψ)s(θ) I RB (φ, θ, ψ) = c(θ)s(ψ) c(φ)c(ψ) + s(φ)s(ψ)s(θ) c(φ)s(ψ)s(θ) − c(ψ)s(φ) −s(θ) c(θ)s(φ) c(φ)c(θ) (A.3)

53

Appendix A. Math

54

Bibliography [1] Ascending Technologies GmbH, AscTec Hummingbird with AutoPilot, User’s Manual, 2012. http://www.asctec.de/downloads/manuals/ AscTec-Autopilot-Manual-v10.pdf. [2] Ascending Technologies GmbH, Institute of Flight System Dynamics, Technical University Munich, AscTec Simulink toolkit, Manual V1.01, 2012. http://www.asctec.de/downloads/software/simulink_toolkit/ asctec_simulink_toolkit_manual.pdf. [3] K. Karwoski, “Internship Final Report, Quadrocopter Control Design and Flight Operation.” NASA USRP, 2011. [4] M. Achtelik, “Nonlinear and Adaptive Control of a Quadcopter,” Master’s thesis, Institute of Flight System Dynamics, Technical University Munich, 2010. [5] MathWorks, Fixed-Point Designer, User’s Guide, r2012b ed., 2012. http: //www.mathworks.com/help/pdf_doc/fixpoint/fp_blks.pdf. [6] S. K. Phang, C. Cai, B. Chen, and T. H. Lee, “Design and mathematical modeling of a 4-standard-propeller (4SP) quadrotor,” in Intelligent Control and Automation (WCICA), 2012 10th World Congress on, pp. 3270–3275, 2012. [7] D. Mellinger, Q. Lindsey, M. Shomin, and V. Kumar, “Design, modeling, estimation and control for aerial grasping and manipulation.,” in IROS, pp. 2668–2673, IEEE, 2011. [8] T. Bresciani, “Modelling, Identification and Control of a Quadrotor Helicopter,” Master’s thesis, Lund University, Department of Automatic Control, 2008. [9] R. M. Moses Banguar, “Nonlinear Dynamic Modeling for High Performance Control of a Quadrotor,” Proceedings of Australasian Conference on Robotics and Automation, 2012. [10] V. A. M. Yasir Amir, “Modeling of Quadrotor Helicopter Dynamics,” International Conference on Smart Manufacturing Application, 2008. [11] J. Slotine and W. LI, Applied Nonlinear Control. Prentice Hall, 1991. [12] H. Khalil, Nonlinear Systems. Prentice Hall PTR, 2002. [13] P. Ioannou and B. Fidan, Adaptive Control Tutorial. Advances in Design and Control, Society for Industrial and Applied Mathematics, 2006. [14] M. A. Duarte and K. Narendra, “Combined direct and indirect approach to adaptive control,” Automatic Control, IEEE Transactions on, vol. 34, no. 10, pp. 1071–1075, 1989. 55

Bibliography

56

[15] M. A. Duarte and K. S. Narendra, “Error models with parameter constraints,” International Journal of Control, vol. 64, no. 6, pp. 1089–1111, 1996. [16] G. Chowdhary and E. N. Johnson, “Concurrent learning for convergence in adaptive control without persistency of excitation.,” in CDC, pp. 3674–3679, IEEE, 2010. [17] N. A. Chaturvedi, D. S. Bernstein, J. Ahmed, F. Bacconi, and N. H. McClamroch, “Globally convergent adaptive tracking of angular velocity and inertia identification for a 3-DOF rigid body,” IEEE Trans. Contr. Sys. Techn., vol. 14, no. 5, pp. 841–853, 2006. [18] R. Leine, “Nonlinear Dynamics and Chaos I.” ETH Zurich, Class notes, 2011. [19] J. K. Hedrick and A. Girard, “Control of Nonlinear Dynamic Systems: Theory and Applications.” Berkeley University of California, Class notes, 2010. [20] N. Hovakimyan and C. Cao, L1 Adaptive Control Theory: Guaranteed Robustness with Fast Adaptation. Advances in Design and Control, Society for Industrial and Applied Mathematics (SIAM, 3600 Market Street, Floor 6, Philadelphia, PA 19104), 2010. [21] K. W. Eugene Lavretsky, Robust and Adaptive Control: With Aerospace Applications. Springer, 2013. [22] M. Imran Rashid and S. Akhtar, “Adaptive control of a quadrotor with unknown model parameters,” in Applied Sciences and Technology (IBCAST), 2012 9th International Bhurban Conference on, pp. 8–14, 2012.