Robust Control of Teleoperated Unmanned Aerial Vehicles

IN

DEGREE PROJECT ELECTRICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2020,

Robust Control of

Teleoperated Unmanned Aerial Vehicles

CHUNYANG HAN

KTH ROYAL INSTITUTE OF TECHNOLOGY

A BSTRACT

In this thesis, we first use the reachability theory to develop algorithms for state predic- tion under delayed state or output measurements. We next develop control strategies for collision avoidance and trajectory tracking of UAVs based on the devised algorithms and the model predictive control theory. Finally, simulations results for collision avoidance and trajectory tracking problems are presented, for different communication delays, using a UAV model with 6 degrees of freedom.

Keyword

communication delay, Model Predictive Control, linear discrete control system, reacha- bility of target set and target tube

S AMMANFATTNING

I denna avhandling använder vi först tillgänglighetsteorin för att utveckla algoritmer för tillståndsförutsägelse under fördröjda tillstånds- eller utgångsmätningar. Därefter utvecklar kontrollstrategier för undvikande av kollision och spårning av UAV: er baser- ade på de planerade algoritmerna och modellen förutsägbar kontrollteori. Slutligen presenteras simuleringsresultat för att undvika kollision och problem med spårning av banan, för olika kommunikationsförseningar, med en UAV-modell med 6 frihetsgrader.

Keywords

kommunikationsfördröjning, Model Predictive Control, linjärt diskret styrsystem, mål- sättning och målrörs tillgänglighet

T ABLE OF C ONTENTS

Page

List of Tables vii

List of Figures ix

1 Introduction 1

1.1 Motivation . . . 1

1.2 Related Work . . . 2

1.2.1 Reachability Analysis of Linear System . . . 2

1.2.2 Control of UAV under Delay . . . 3

1.3 Objectives . . . 3

1.4 Contributions . . . 4

2 Theory 7 2.1 State Prediction under Communication Delay . . . 7

2.1.1 State Estimation Using Initial Condition . . . 8

2.1.2 State Prediction with Delayed State Measurements . . . 8

2.1.3 State Prediction with Delayed Output Measurements . . . 11

2.2 Comparison between State Estimation Using Initial Condition and State Prediction with Delayed State Measurements . . . 13

3 Application 15 3.1 Background . . . 15

3.1.1 Model Predictive Control . . . 15

3.1.2 Reachability Analysis Toolbox . . . 20

3.1.3 Optimization Modeling Toolbox . . . 21

3.2 Algorithm . . . 21

3.3 Simulation . . . 25

TABLE OF CONTENTS

3.3.1 Collision Avoidance with Communication Delay . . . 25 3.3.2 Tracking Problem with Communication Delay . . . 27

4 Conclusion and Future Development 33

Bibliography 35

A Appendix 37

A.1 Computation for Hyperplanes . . . 37

vi

L IST OF T ABLES

TABLE Page

3.1 Parameters for collision avoidance with communication delay . . . 25

L IST OF F IGURES

FIGURE Page

1.1 Time delayed control system in UAV . . . 4

2.1 Delayed state information . . . 9

2.2 Discrete-time UAV system using initial condition . . . 14

2.3 Discrete-time UAV system with delayed information . . . 14

3.1 MPC: receding horizon principle [1] . . . 16

3.2 MPC (or RHC): computation timeline [2] . . . 19

3.3 Reachable set with hyperplane . . . 26

3.4 Collision avoidance examples . . . 26

3.5 Video screenshots for different tracking problems . . . 27

3.6 Tracking problem 1 (delayed timeτu= 1 , τd= 1 ) . . . 28

3.7 Tracking problem 2 (delayed timeτu= 2 , τd= 2 ) . . . 29

3.8 Tracking problem 3 (delayed timeτu= 3 , τd= 1 ) . . . 30

3.9 Tracking problem 4 (delayed timeτu= 1 , τd= 1 ) . . . 31

A.1 Notation of an ellipsoid . . . 38

A.2 Hyperplane examples . . . 38

L IST OF A LGORITHMS

1 State prediction with delayed state measurements . . . 10

2 State prediction with delayed output measurements . . . 13

3 Algorithm with MPC in collision avoidance problem . . . 23

4 Algorithm with MPC in tracking problem . . . 23

5 Algorithm with MPC and state prediction . . . 25

C

1

I NTRODUCTION

1.1 Motivation

Unmanned aerial vehicles (UAVs) are flying robots which can be controlled by a human operator or a remote controller while teleoperated unmanned aerial vehicles (TUAVs) are flying robots which can be controlled remotely while flying. When it comes to drones for entertainment purpose, generally they can fly within a short time and a short control distance. However, the advanced teleoperated unmanned aerial vehicles which can be controlled from a relatively long distance are equipped with excellent flight capabilities because of their light weight, flexible shape and other advantageous properties. They can be used in remote surveillance applications which demand them to hover for hours and can receive controlled information from a very long distance.

TUAVs are also allowed to be equipped with many advanced equipment, such as GPS guided missiles, sensors, cameras, navigation systems and Global Positioning Systems (GPS), which also improve their capability for locating and detecting in remote surveillance situations. For example, in firefighting situation, TUAVs are used to test the amount of CO, CO₂, etc gases in air with certain equipment [3]. In research work, they help scientists to investigate different kinds of occurrences in a certain environment that is dangerous and inaccessible for human operators and other robots. For instance, they are implemented in measuring contamination in nuclear accidents, observing volcanic eruptions, etc [3].

In these applications, communication links convey control commands and sensors’

CHAPTER 1. INTRODUCTION

measurements between TUAVs and remote controllers or human operators. As a result, the closed-loop performance of systems comprising TUAVs highly depends on the quality of the communication links. For example, the performance heavily degrades in presence of time-varying communication delays and packet drops. The idea of this thesis is to improve the closed-loop performance of these systems against uncertainties imposed by communication links. The more precise the time-delayed control model is, the better the closed-loop performance of systems will be.

1.2 Related Work

The research for teleoperated unmanned aerial vehicles has become more and more popular in recent years and the topic of designing time delayed control system has been discussed in many research articles. This literature provides different models from different dynamic systems to solve this time delayed problem.

1.2.1 Reachability Analysis of Linear System

The uncertainty can be disturbance in the dynamics of system, and disturbances can affect the state or output measurements. In uncertain dynamic systems, the feedback control may be generated under a condition that the system state is restricted to stay in a specific region of state space, and the state space can only be expressed by input and observation disturbances [4]. In the paper [4], the closed-loop control of discrete-time systems with uncertainty is discussed. In the first part, the paper talks about under the worst disturbances of system dynamics, how to put the system state at the final time in a given set, which is called a target set. In the second part, the paper gives a target tube, and talks about the approach to keeping the system state trajectory in the target tube.

In the last part, the paper shows the sufficient and necessary conditions for reachability of a target set and a target tube, as long as the system state can be measured exactly [4].

According to paper [5], the reachability analysis for nonlinear dynamical systems can also be achieved by solving appropriate Hamilton-Jacobi equations. This approach provides a very efficient tool to deal with many applications regarding reachability, such as path planning with collision avoidance. Classic schemes of solving Hamilton-Jacobi equation are based on Finite Differences, Semi-Lagrangian approximations which are generally used for solving Partial Diffential Equation (PDE) and so on. The solutions of these schemes can be very stable.

2

1.3. OBJECTIVES

In paper [6], the approach to computing reachable sets for discrete time linear control systems is described. For the discrete time control system with time-varying coefficients, the computation of the reachable sets can be approximated by ellipsoidal bounds from both initial conditions and feedback controls of the system. In paper [7], it provides a way to compute the ellipsoid more precisely. The paper constructs external and internal ellipsoidal approximation which can touch the reachable sets boundary from both inside and outside. For implementation of computing reachable sets, "ellipsoidal tool" package in MATLAB is designed to generate the approximate reachable ellipsoidal sets [8].

1.2.2 Control of UAV under Delay

Because of delayed state information and outdated control feedback, the stability of the UAV control system will be affected by communication delay. Although many of time delayed control algorithms have been came up with to solve the communication delay problem, most of studies have assumed that the communication delay is constant or the system dynamics is known and simple. But because of UAVs’ time-sensitive and nonlinearity characteristics, the assumptions are not proper to apply for UAVs’ systems.

In paper [9], it came up with a time delayed control system under communication delay by using Model Predictive Control (MPC). It is then extended to learn an unknown nonlinear model by using Gaussian Process (GP), which increase the accuracy of computing state estimation and trajectory planning under communication delay.

Furthermore, the paper [10] also provides different ways to build up a networked control model under communication delay. In this paper, it proposes two time delayed models for multi-rotor system under communication delay. The first one is using linear robust MPC with disturbance and the second one is using nonlinear MPC without disturbance. By implementing state estimation to the MPC method with Robot Operating System (ROS) [10], this paper also provides a way to achieve trajectory tracking for both multi-rotor UAV and fixed-wing UAV. In the paper [11], it describes a linear system without disturbance and proposes a block diagram model which exploit the structure of uncertainty to compensate the impact of communication delay.

1.3 Objectives

We consider the remote control of a plant over a common custom network as shown in Figure 1.1, The controller receives sensor measurements with delay and the actuator

CHAPTER 1. INTRODUCTION

receives the controller commands with delay. The objective of this thesis is to improve the closed-loop performance of UAVs against communication delay. This is achieved by developing state prediction algorithms with delayed state or output measurements.

By estimating forward state prediction with delayed state measurements and using Linear Model Predictive Control method, we compensate the impact of uncertainties imposed by communication links. The model is used to implemented two applications, such as collision avoidance under communication delay and tracking problem under communication.

Figure 1.1: Time delayed control system in UAV

1.4 Contributions

The contributions of this thesis are organized as follows:

1. Chapter 2 introduces the theory for predicting reachable sets under delayed state or output measurements.

2. In Chapter 3, the algorithms which are used for predicting state sets and also form- ing trajectories of UAVs are implemented to two applications, including collision avoidance with communication delay in two UAVs and tracking problems in two

4

1.4. CONTRIBUTIONS

UAVs with communication delay. Furthermore, simulation results for the work above are also showed.

3. In Chapter 4, conclusions based on the simulation results and suggestions for future work are discussed.

C

2

T HEORY

In this Chapter, we study the state estimation problem under communication delay, for discrete time system with state or output measurements.

2.1 State Prediction under Communication Delay

Let us consider the discrete-time system:

(2.1) x_k+1= Akx_k+ Gkw_k

where A_k∈ R^n∗n, G_k∈ R^n∗p, state vector x_k∈ Xk⊂ Rⁿ(n-dimensional Euclidean space) and the disturbance w_k is assumed to belong to a given bounded set W_k⊂ R^p. This is the system we use for state prediction. Notice that we introduce the state equations but there is no input in this system.

CHAPTER 2. THEORY

2.1.1 State Estimation Using Initial Condition

If we don’t know any information from the UAV plant, we assume the initial state set X₀ is given, then

Xˆ₀= X0,

Xˆ₁= A0Xˆ₀⊕ G0W₀, Xˆ₂= A1Xˆ₁⊕ G1W₁, ...

Xˆ_N= A_N−1Xˆ_N−1⊕ G_N−1W_N−1 (2.2)

where ˆX₀, ˆX₁, ˆX₂, ... ˆX_N are estimated reachable state sets [4] at time step t=0,1,2,...N, respectively, and W_k is bounded set of disturbance. Notice that "⊕" here and in the following equations corresponds to set addition.

In this case, we can get the estimated sets of possible states. But as we know that the X₀, G, W_k are all given bounded sets, the size of estimated reachable sets will increase with time, meaning that the estimator with only initial information is not accurate enough.

2.1.2 State Prediction with Delayed State Measurements

In this subsection, we assume that the estimator only has access to the delayed state measurements. Our objective is to design a state estimator with delayed state measure- ments.

Under communication delay, the estimator can get the corresponding delayed infor- mation at each time step, meaning that the estimator with communication delay can get only part of past state information, which means the estimation with communication delay is totally different from the situation without communication delay. We illustrate the state transmission under communication delay in Figure 2.1.

Next we derive the structure of state estimation with delayed state measurements.

In the case of communication delay, we assume that the delay isτ time steps, and at time step t = N we also assume that the initial state x0is given for both N ≤ τ and N ≥ τ + 1 situations.

For N ≤ τ, the estimator only knows initial state x0. From the linear system in Equa- tion 2.1, we know the corresponding state set at time step t = 0,1,2,...., N, respectively, as is shown in Equation 2.3:

8

2.1. STATE PREDICTION UNDER COMMUNICATION DELAY

Figure 2.1: Delayed state information

Xˆ₁= A0x₀⊕ G0W₀, Xˆ₂= A1Xˆ₁⊕ G1W₁, ...

Xˆ_N= A_N−1Xˆ_N−1⊕ G_N−1W_N−1 (2.3)

where ˆX₁, ..., ˆX_N−1, ˆX_N are estimated reachable state sets at time step t = 1,..., N − 1, N, respectively, and W₀, W₁, ..., W_N−1 are bounded set of disturbance at time step t = 0,1,..., N − 1, respectively.

For N ≥ τ + 1, the estimator has information from t = 0,1,2,...., N − τ. From the linear system in Equation 2.1, we know the corresponding state at time step t = 0,1,2,...., N − τ, respectively, which can be written as Equation 2.4:

x₁= A0x₀+ G0w₀, x₂= A1x₁+ G1w₁, ...

x_N−τ= A_N−τ−1x_N−τ−1+ G_N−τ−1w_N−τ−1. (2.4)

To find the estimated reachable set at t = N, we compute the reachable sets with the given state measurements x₀, x₁, x₂, ...., x_N−τ. As is shown in Equation 2.5:

CHAPTER 2. THEORY

Xˆ_N−τ+1= A_N−τx_N−τ⊕ G_N−τW_N−τ,

Xˆ_N−τ+2= A_N−τ+1Xˆ_N−τ+1⊕ G_N−τ+1W_N−τ+1, ...

Xˆ_N= A_N−1Xˆ_N−1⊕ G_N−1W_N−1 (2.5)

where ˆX_N−τ+1, ˆX_N−τ+2, ..., ˆX_N−1, ˆX_N are estimated reachable state sets at time step t = N − τ + 1, N − τ + 2,..., N − 1, N, respectively, and W_N−τ, W_N−τ+1, ..., W_N−1 are bounded set of disturbance at time step t = N − τ, N − τ + 1,..., N − 1, respectively.

From Equation 2.5, it is inferred that we can use the given state x_N−τ to describe the estimated reachable set at time step t = N. As we can see in Equation 2.6, for state estimation with communication delay, the estimated reachable set ˆX_N depends on the latest information the estimator receives according to delayed time steps τ and the disturbance of the system from time step t = N − τ, N − τ + 1,..., N − 1 when N ≥ τ + 1.

A^(n,τ)=

N−1Y

i=N−(τ−n)

A_i,

Xˆ_N= A^(0,τ)x_N−τ⊕ A^(1,τ)G_N−τW_N−τ⊕ A^(2,τ)G_N−τ−1W_N−τ−1⊕ ....

⊕ A^(τ−2,τ)G_N−3W_N−3⊕ A^(τ−1,τ)G_N−2W_N−2⊕ G_N−1W_N−1. (2.6)

Equation 2.6 above expresses the structure of estimator with delayed state mea- surements. Next, we create an algorithm structure for this problem, as is showed in Algorithm 1:

Algorithm 1: State prediction with delayed state measurements

1 τ ← delayed time steps;

2 x₀← initial state;

3 if N ≥ τ + 1 then

4 x_N−τ← D iscretes ys_state(x0,τ);

5 Xˆ_N← P rediction_state(x_N−τ);

6 else

7 Xˆ_N← P rediction_stateN ≤ τ(x0);

8 end

where "Discretesys _ state" is corresponding to Equation 2.1 and 2.4, "Prediction_state"

is corresponding to Equation 2.6, and "Prediction_stateN≤ τ" is corresponding to Equation 2.3.

10

2.1. STATE PREDICTION UNDER COMMUNICATION DELAY

2.1.3 State Prediction with Delayed Output Measurements

In this subsection, we assume that the estimator only has access to the delayed out- put measurements. Our objective is to design a state estimator with delayed output measurements.

Now let us concentrate on the case of the linear discrete system with output measure- ments:

x_k+1= Akx_k+ Gkw_k z_k= Hkx_k+ vk

(2.7)

where A_k∈ R^n∗n, G_k∈ R^n∗p, H_k∈ R^m∗n, input disturbance w_k∈ Wk⊂ R^p and measure- ment disturbance v_k∈ Vk⊂ R^m are all given and x_k∈ Xk⊂ Rⁿwith the initial condition x₀contained in a given set X0⊂ Rⁿ.

In the absence of communication delay, the set S⁰

k|kof estimated states x_k, consistent with a given set of output measurements z₁, z₂, z₃, ...., z_kis given recursively by Equation 2.8 [4]:

S⁰

k|k= S⁰_k|k−1∩ {xk: z_k− Hkx_k∈ Vk}, S⁰_k|k−1= A_k−1S⁰_k−1|k−1⊕ G_k−1W_k−1,

S⁰_0|0= X0, k = 1,2,.., N − 1, N.

(2.8)

In the case of communication delay, we assume that the delay isτ time steps, and at time step t = N we also assume that the initial state set X0is given for both N ≤ τ and N ≥ τ + 1 situations.

For N ≤ τ, since the estimator only knows the initial state set X0, we can use the method in Equation 2.8 to estimate the set S_k|0of estimated states x_k in k = 1,2,.., N.

As is shown in Equation 2.9:

S_0|0= X0,

S_1|0= A0S_0|0⊕ G0W₀, S_2|0= A1S_1|0⊕ G1W₁,

....

S_N|0= A_N−1S_N−1|0⊕ G_N−1W_N−1. (2.9)

CHAPTER 2. THEORY

For N ≥ τ + 1, since the output measurements z1, z2, z3, ...., z_N−τ are known, we can use the method in Equation 2.8 to estimate the set S_k|k of estimated states xk in k = 1,2,.., N − τ. The state set estimation can be expressed by:

S_0|0= X0,

S_1|0= A0S_0|0⊕ G0W₀,

S_1|1= S_1|0∩ {x1: z₁− H1x₁∈ V1}, ....

S_k|k−1= A_k−1S_k−1|k−1⊕ G_k−1W_k−1,

S_k|k= S_k|k−1∩ {xk: z_k− Hkx_k∈ Vk}, k = 1,2,.., N − τ.

(2.10)

In k = N − τ + 1,..., N, the set Sk|N−τof estimated states xk can be obtained by:

S_{N−τ+1|N−τ}= A_N−τS_{N−τ|N−τ}⊕ G_N−τW_N−τ,

S_{N−τ+2|N−τ}= A_N−τ+1S_{N−τ+1|N−τ}⊕ G_N−τ+1W_N−τ+1, ....

S_N|N−τ= A_N−1S_N−1|N−τ⊕ G_N−1W_N−1. (2.11)

Then for N ≥ τ + 1, we can summarize the set S_N|N−τ of estimated states x_N given the information of output measurements z₁, z₂, z₃, ...., z_k−τ, as is shown in Equation 2.12:

A^(n,τ)=

N−1Y

i=N−(τ−n)

A_i,

S_N|N−τ= A^(0,τ)S_{N−τ|N−τ}⊕ A^(1,τ)G_N−τW_N−τ⊕ A^(2,τ)G_N−τ−1W_N−τ−1⊕ .... ⊕ A^(τ−1,τ)G_N−2W_N−2⊕ G_N−1W_N−1.

(2.12)

Equation 2.12 above expresses the structure of estimator with delayed output mea- surements. Next, we present an algorithm structure for this problem, as is showed in Algorithm 2:

12

2.2. COMPARISON BETWEEN STATE ESTIMATION USING INITIAL CONDITION AND STATE PREDICTION WITH DELAYED STATE MEASUREMENTS

Algorithm 2: State prediction with delayed output measurements

1 τ ← delayed time steps;

2 X₀← initial state set;

3 if N ≥ τ + 1 then

4 z₁, z₂, z₃, ...., z_k−τ← given set o f output measurements;

5 S_{N−τ|N−τ}← D iscretes ys_output(X0, z₁, z₂, z₃, ...., z_k−τ,τ);

6 S_N|N−τ← P rediction_output(S_{N−τ|N−τ});

7 else

8 S_N|0← P rediction_outputN ≤ τ(X0);

9 end

where "Discretesys _ output" is corresponding to Equation 2.10 and Equation 2.7,

"Prediction_output" is corresponding to Equation 2.12, and "Prediction _ outputN≤ τ" is corresponding to Equation 2.9.

2.2 Comparison between State Estimation Using Initial Condition and State Prediction with Delayed State Measurements

Here we demonstrate the differences between state estimation using initial condition as we deduced in Equation 2.2 and state estimation with delayed state measurements as we deduced in Equations 2.4, 2.5 and 2.6.

Under the same dynamics and the same initial conditions, we estimate state reachable sets by initial condition as we can see in Figure 2.2, compared with state reachable sets estimation by delayed state information (communication delay isτ = 4) as we can see in Figure 2.3.

Because ofτ = 4, after time step = 4, the state estimations become larger and larger in Figure 2.2 estimated by initial conditions, while state estimations keep the same area in Figure 2.3 because the states are predicted by updated delayed state information. In a conclusion, the state estimation with delayed state measurement is more accurate than the state estimation using initial condition.

CHAPTER 2. THEORY

Figure 2.2: Discrete-time UAV system using initial condition

Figure 2.3: Discrete-time UAV system with delayed information

14

C

3

A PPLICATION

In this Chapter, we firstly introduce Model Predictive Control theory, YALMIP, MPT toolbox and ellipsoid toolbox. All of toolboxes are implemented in MATLAB R2017b. Then the algorithm and simulation results of two application examples are demonstrated.

3.1 Background

3.1.1 Model Predictive Control

Model Predictive Control (MPC) technique uses the dynamical model of the process to predict its output or state values and optimize the control signal. MPC computes the control signal by solving a finite-horizon optimization control problem at every sampling instant while satisfying a set of constraints. So far, MPC has been applied successfully to a variety of applications. For example, it can be used in the technology for advanced multivariable control applications such as trajectory optimization and tracking of drones and fine-tune steering of autonomous trucks. There are also many practical advantages that MPC can bring:

• Straightforward formulation and constraint enforcement based on well understood concepts

• Easy to tune parameters – Prediction horizon

CHAPTER 3. APPLICATION

– Parameters for optimization problem itself, such as the initial condition and system matrices.

• Computation time much shorter than most of other advanced control methods

MPC is also known as Dynamic Matrix Control (DMC), Generalized Predictive Control (GPC), and Receding Horizon Control (RHC). The basic idea behind MPC is to plan an optimal control sequence over a fixed horizon N by minimizing an objective function. At the initial step the control input is implemented to the dynamics of system, with the sampling system states, new control actions and new predicted states are generated. Then the computation of getting new control actions and new predicted sequence of states are repeated. Because the prediction horizon keeps moving forward, MPC is also called receding horizon control. [12] The receding horizon principle is illustrated in Figure 3.1.

Figure 3.1: MPC: receding horizon principle [1]

Unfortunately, even for linear systems and polyhedral state and control constraints, the optimal control policy is difficult to find. So instead of relying on dynamic program- ming and aiming for optimal policies, we will use the receding horizon principle to find policies that guarantee stability and have near-optimal performance [2]. Although this is not optimal, it has given very good results in practice. Nowadays, many of researchers in control field are still looking for faster and more stable methods to improve the MPC method.

16

3.1. BACKGROUND

3.1.1.1 General Formulation

The discrete dynamics of a system can be written as:

(3.1) x(k + 1) = fk(x(k), u(k)), t ≤ 0.

Denote the number of states to be n and the number of control inputs to be m, so x(k) ∈ Rⁿ, u(k) ∈ R^m. At every time k, the MPC plans an optimal control sequence over a fixed horizon N. Here we introduce the notations:

(3.2) x(k + i|k), u(k + i|k), k = 0,1,.., N, i = 0,1,..N

for the prediction of system state and control actions at time k+i, given the information available about the system state and input at time t. The notations in Equation 3.2 are written by x_k+i|k, u_k+i|k shortly. Notice that the predicted states and control actions x_k+i|k, u_k+i|kshould be seen as possible future states and inputs over possible trajectories, they should be distinguished by the real states x(k) and control inputs u(k), denoted by x_k, u_k respectively.

Let us denote the sequences of states and control actions:

x_k= {x_k+1|k, x_k+2|k, .., x_k+N|k}, u_k= {u_k|k, u_k+1|k, .., u_k+N−1|k}.

(3.3)

We also need the system’s states and control inputs to satisfy the MPC’s constraints , which can be expressed generally as belonging to the sets:

x₀∈ X0⊆ Rⁿ, x_k∈ X ⊆ Rⁿ, u_k∈ U ⊆ R^m. (3.4)

3.1.1.2 Linear MPC

The discrete dynamics of system is written as:

(3.5) x_k+1= Akx_k+ Bku_k

where state vector x_k∈ Rⁿ, control input vector u_k∈ R^m, and A ∈ R^n∗n, B ∈ R^n∗m. Then the MPC can be expressed as a linear MPC and the system state and input constraints are also assumed to be linear. Constraints 3.4 can be expressed using matrices M_x, Muand vectors vx, vuas:

M_xx_k+i≤ Vx, i = 1,2,.., N − 1 M_uu_k+i≤ Vu, i = 1,2,.., N − 1.

(3.6)

CHAPTER 3. APPLICATION

In addition, we impose an extra constraint on the terminal state by using matrix MN

and vector VN, denoted by

(3.7) M_Nx_k+N≤ VN.

The objective function which is also called cost function is defined as a function of the initial state x0and the control input sequence {u_k, u_k+1, .., u_k+N−1},

(3.8) V_N(x0, u) =

N−1X

i=0

J(x_k+i, u_k+i) + JN(x_k+N)

where the stage cost function J(xk, uk) is denoted to be a convex function.

In the linear case, we choose the stage cost function to be a linear quadratic function, denoted by

(3.9) J(x_k, u_k) = x^T_kQ x_k+ u^T_kRu_k, J_N(x_k+N) = x^T_k+NQ_Nx_k+N

where Q and QN are positive semidefinite matrices and R is a positive definite matrix.

Then the linear MPC problem is to solve the optimization problem 3.10 with optimal solution, which if exists, are a optimal state sequence and a optimal control action sequence.

(3.10)

minimize

u

N−1X

i=0

J(x_k+i|k, u_k+i|k) + JN(x_k+N|k) subject to x_k|k= x0,

x_k+i+1|k= Akx_k+i|k+ Bku_k+i|k, i = 1,..., N − 1 M_xx_k+i|k≤ Vx, i = 1,..., N − 1

M_uu_k+i|k≤ Vu, i = 1,..., N − 1 M_Nx_k+N|k≤ VN.

Regarding the definition of state and control input sequences 3.3, the optimal control action sequence and its corresponding state sequence can be showed as:

u^∗_k= {u^∗_k|k, u^∗_k+1|k, .., u^∗_k+N−1|k}, x^∗_k= {x^∗_k+1|k, x^∗_k+2|k, .., x^∗_k+N|k} (3.11)

where the first control action is applied as a real input, such as:

(3.12) u_k= u^∗_k|k.

18

3.1. BACKGROUND

Further, the cost function evaluated using the optimal control input sequence is called the value function:

(3.13) V_N⁰(x₀) = VN((x₀, u^∗) where the optimal control input sequence is denoted as:

(3.14) u^∗= {u^∗_1|1, u^∗_2|2, u^∗_3|3, .., u^∗_k|k, u^∗_k+1|k+1 , ...}.

In addition, the initial condition at each time sample is given by the corresponding state of system x0= x(k). The whole computation timeline can be described in Figure 3.2 as following.

Figure 3.2: MPC (or RHC): computation timeline [2]

3.1.1.3 Practical Aspects For Implementing MPC

Notice that when implementing MPC, we need to consider many practical aspects, such as

• Estimating the future states as precise as possible

• Keeping computational time as small as possible

• Keeping the optimization problem feasible

We can measure the accuracy of estimated future states by comparing the estimated trajectories with the real trajectories that we get from experiments. The sampling time

CHAPTER 3. APPLICATION

and the number of modelled states can both affect the accuracy of estimated future states.

The main factors that can influence the running time of the problem are the number of modelled states and the control inputs as well as the time horizon (sampling time).

Furthermore, the type of the optimization problem (linear, nonlinear, convex, nonconvex) can also affect the computational time. In order to limit the computational time, we need to keep the number of modelled states relatively small and the time horizon length may not need to be very long depending on the dynamic system itself.

Feasibility can be influenced by the terminal set, the horizon length N and the state and input constraints. If the size of terminal set is chosen too small, or the horizon length is too short, then the set of feasibility will be small. Then the solutions may be too conservative. For example, part of the trajectory states keeps returning to a certain small area or the terminal set when the small terminal set or short horizon is applied to the collision avoidance problem. If we choose the inappropriate terminal set which is infeasible for the constraints sets, or if we choose a short or a long time horizon length, the optimization problem will be infeasible. States constraints can also cause infeasibility due to disturbance or measurements noise and so on. Then the solver will not be able to compute a valid control action. As a result, the optimality of Model Predicted Control will be lost.

In our topic, adding relaxation to the state and input constraints can make the controlled UAV more safe and further from the uncontrolled UAV, but it may increase the total energy cost for the system, such as the cost function of the optimization problem.

3.1.2 Reachability Analysis Toolbox

Reachability analysis is recognized as a problem to compute a reachable set in a more effectively way. It requires that in a given time, the approach can determine if the reachable set have nonempty intersection with a given set (target set), and projection of the reachable set can be displayed into two-or three-dimensional subspace [8]. In MATLAB, reachable sets can be analyzed and calculated by Ellipsoidal Toolbox. With ellipsoidal calculus, the reachable set of linear system can be approximately measured by implementing internal and external ellipsoids.

In our topic, the disturbance W and initial state set X₀talked in Chapters 2 and 3 are all given bounded sets. So in our implementation, we set the given bounded sets to be ellipsoids, such as in Equations 2.4, 2.5 and 2.6. We choose to estimate the reachable sets ˆX_k, k = τ,τ + 1,τ + 2,...., by Ellipsoid Toolbox implementing in MATLAB R2017b.

20

3.2. ALGORITHM

3.1.3 Optimization Modeling Toolbox

Linear matrix inequalities (LMI) and semidefinite programming (SDP) are two impor- tant mathematical tools in control and systems theory. Semidefinite programming can deal with many of control problems, including classical Lyapunov theory for linear sys- tems, modern control theory based on the algebraic Riccati equation and so on. LMIs and SDP has contributed a lot to many new developments on robust model predictive control,stability analysis and so on [13].

SDPs is concerned with a convex optimization problems that can be dealt with in polynomial time in 1990s. After that, software with modeling language and interface such as YALMIP (Yet Another LMI(linear matrix inequality) Parser) Toolbox are developed for SDP. Nowadays, YALMIP Toolbox is a modeling language package in MATLAB designed for supporting linear programming (LP), quadratic programming (QP), semidefinite programming, determinant, multiparametric linear and so on. YALMIP Toolbox has many benefits, for example, it can automatically reduce the the number of variables in equality constrained problems [13].

In the section 3.1.1, we introduced Model Predictive Control optimization method to optimize the problem for collision avoidance under communication delay and tracking problem under communication delay. However, since MPC is based on optimization control theory, it needs a considerable amount of on-line computer power to deal with the optimization problems with a low enough time cost [13]. To solve this problem, a concept of multiparametric programming is introduced, which means generating explicit solutions to parameterized optimization problems. The algorithm of multiparametric programming can calculate explicit solutions efficiently by using less on-line computer resources and has been publicly in MPT Toolbox in MATLAB [14]. MPT toolbox is also interfaced in YALMIP Toolbox. With MPT and YALMIP Toolbox implemented in MATLAB, we can design the MPC controller to solve our optimization problem by using multiparametric programming and semidefinite programming solvers [13].

3.2 Algorithm

In this section, we discuss two applications including collision avoidance under commu- nication delay and tracking problem under communication delay. We define that

1. τu is the uplink time delay andτd is downlink time delay.

CHAPTER 3. APPLICATION

2. U AV^cis the UAV that we can control and U AV^ucis the UAV which is uncontrolled.

The dynamics of both UAVs follows the linearized dynamic model of quadrotor in [15]. The discrete-time control system of U AV^c(3.15) is same as Equation 3.5,

(3.15) x^c

k+1= A^c_kx_k^c+ B_k^cu^c_k

where state vector x_k^c ∈ Rⁿ , control input vector u^c_k∈ R^m, and A_k^c ∈ R^n∗n, B^c_k∈ R^n∗m.

The discrete-time system of U AV^uc (3.16) is same as Equation 2.1

(3.16) x^uc

k+1= A^uc_k x^uc_k + G^uc_k w_k^uc.

where state vector x_k^uc∈ Rⁿ , A^uc_k ∈ R^n∗n, G^uc_k ∈ R^n∗p, and the disturbance w_k is assumed to belong to a given bounded set W_k⊂ R^p.

We also assume that 1. (τu,τd) are known.

2. At time t=0, the states of both UAVs are known at the controller.

3. The U AV^c starts moving after receiving the first command.

4. We generate the first command at time t=0.

In collision avoidance problem, we consider two UAVs, U AV^cand U AV^uc. Once the U AV^ucstarts flying, we receive its position information with a communication downlink delay τd. Then we send the state estimator information and the control information out to the controlled U AV^cwith a communication uplink delayτu to control U AV^cto avoid the trajectory of U AV^ucwhile both are flying. Notice that the terminal position of U AV^cis given .

In tracking problem under communication delay, we also consider two UAVs, U AV^c and U AV^uc. Once the uncontrolled U AV^uc starts flying, we receive its position informa- tion with a communication downlink delayτdand send the control information out to the U AV^c with a communication uplink delayτu to track U AV^uc’s trajectory. Notice that we require the U AV^cto start flying at the U AV^uc’s initial state at time t=0, then track the U AV^uc’s trajectory without crashing.

22

3.2. ALGORITHM

Algorithm 3: Algorithm with MPC in collision avoidance problem

1 k ← 0;

2 x₀^c← x^c(0);

3 while k ∈ time horizon do

4 h_k is given ;

5 x^c∗

k+1, u^c∗_k ← MPC_ Avoidance(x₀^c, h_k);

6 if feasible solution then

7 u^c(k) ← u^c∗_k (0);

8 x^c(k + 1) ← x^c∗_k+1;

9 k ← k + 1;

10 x₀^c← x^c(k);

11 else

12 print( "stop" );

13 break;

14 end

15 end

16

Algorithm 4: Algorithm with MPC in tracking problem

1 k ← 0;

2 x₀^c← x^c(0);

3 while k ∈ time horizon do

4 h_k is given ;

5 x^c∗

k+1, u^c∗_k ← MPC_ Tracking(x^c₀, h_k);

6 if feasible solution then

7 u^c(k) ← u^c∗_k (0);

8 x^c(k + 1) ← x^c∗_k+1;

9 k ← k + 1;

10 x₀^c← x^c(k);

11 else

12 print( "stop" );

13 break;

14 end

15 end

16

CHAPTER 3. APPLICATION

In order to create an algorithm for collision avoidance problem and tracking problem, we create MPC algorithm structures 3 and 4 for the two problems first.

In these two MPC algorithms, {x^c(k), k = 0,1,...} denotes the estimated state trajectory of U AV^c, {u^c(k), k = 0,1,...} denotes the sequence of optimal control regarding U AV^c, {h_k, k = 0,1,...} denotes the hyperplanes of predicted state sets of U AV^uc. Furthermore, both of "MPC_ Avoidance" and "MPC_ Tracking" denotes the optimization problem 3.10 solved at x₀^c= x^c(0). The cost function of "MPC_ Avoidance" and "MPC_ Tracking"

is Equation 3.9 and the constraints of "MPC_ Avoidance" and "MPC_ Tracking" are {h_k, k = 0,1,...}.

Notice that we require different constraints computations in these two MPC algo- rithms. In collision avoidance with communication delay application, we calculate the hyperplanes of U AV^uc’s reachable state sets ˆX^uc

k to restrict the U AV^c’s corresponding state x^c(k) (the calculation method is appended in Appendix A.1). In tracking problem, we require the U AV^c’s state x^c(k) to be the same as that of U AV^uc’s previous states x^uc(i), i = 0,1,.., k −1, and we also calculate the hyperplanes of U AV^uc’s reachable state sets ˆX^uc

k to restrict the U AV^c’s state x^c(k) "behind" U AV^uc’s state x^uc(k). Besides using hyperplanes as safety constraints directly, we can also add more or less relaxation based on the hyperplanes’ coordinates.

Next, we implement the Algorithm 1 from state prediction with delayed state mea- surements in Chapter 2 to the Algorithm 3 from MPC in collision avoidance problem, because Algorithm 1 generates the constraints for Algorithm 3. Then we can get the algorithm with MPC and state prediction in collision avoidance problem. For the same reason, we implement Algorithm 1 to Algorithm 4 from MPC in tracking problem as well, in order to get the algorithm with MPC and state prediction in tracking problem. In the Algorithm 5, we combine the algorithm in collision avoidance problem and the algorithm in tracking problem together.

As we can see in Algorithm 5, {x^uc_k , k = 0,1,...} is given by the U AV^uc’s states informa- tion under communication delay, { ˆX_k^uc, k = 0,1,...} denotes predicted state sets of U AV^uc. Furthermore, "StatePrediction" is corresponding to the Agorithm 1, "SafetyConstrains"

is corresponding to the different constraints computations in Algorithm 3 and Algorithm 4 and "MPC_ AvoidanceorTracking" denotes "MPC_ Avoidance" in Algorithm 3 or "MPC_

Tracking" in Algorithm 4 depending on the problem we will solve.

Using algorithm 5, we get the estimated state trajectory {x^c(k), k = 0,1,...} of U AV^c and predicted state sets trajectory { ˆX^uc

k , k = 0,1,...} of U AV^uc. Then we can achieve the communication delayed control model where the state estimator information and

24

3.3. SIMULATION

Algorithm 5: Algorithm with MPC and state prediction

1 k ← 0;

2 x₀^c← x^c(0);

3 x₀^uc← x^uc(0);

4 while k ∈ time horizon do

5 Xˆ_k^uc← StateP rediction(x₀^uc) ;

6 h_k← Sa f et yConstraints( ˆX_k^uc) ;

7 x^c∗

k+1, u^c∗_k ← MPC_ AvoidanceorTracking(x₀^c, h_k);

8 if feasible solution then

9 u^c(k) ← u^c∗_k (0);

10 x^c(k + 1) ← x^c∗_k+1;

11 k ← k + 1;

12 x₀^c← x^c(k);

13 x₀^uc← x^uc(k);

14 else

15 print( "stop" );

16 break;

17 end

18 end

19

control information are designed to compensate the uncertainties imposed by uplink and downlink communication delay.

3.3 Simulation

3.3.1 Collision Avoidance with Communication Delay

We are aiming at simulating different trajectories with different hyperplane constraints and different time delays. We define the parameters in table 3.1 and apply them to Algorithm 5. Meanwhile, with the hyperplane of reachable sets from uncontrolled UAV in Figure 3.3, we require that the position of the U AV^cis restricted to the shadow area.

Then we simulate the collision avoidance trajectories as is shown in Figure 3.4.

initial position x₀^c terminal position x_N^c initial position x₀^uc time horizon

[-0.5;2] [-20.5;-18] [0;2] 9

Table 3.1: Parameters for collision avoidance with communication delay

CHAPTER 3. APPLICATION

Figure 3.3: Reachable set with hyperplane

communication delay = 1 communication delay = 2

communication delay = 3 communication delay = 4 Figure 3.4: Collision avoidance examples

26

3.3. SIMULATION

As is shown in Figure 3.4, with a fixed time horizon, we observe that more commu- nication delay the system has, the less accurate state estimation will be, which means that the size of reachable set becomes large and the controlled U AV^cneeds to perform unnecessary maneuvers to avoid collision.

3.3.2 Tracking Problem with Communication Delay

In tracking problem, we made 3D animation with four videos by "Videowriter" command in MATLAB R2107b to demonstrate the tracking simulations with different control systems and different communication delays. The video screen shots are showed in Figure 3.5.

Figure 3.5: Video screenshots for different tracking problems

In detail, Figure 3.6 shows the position of U AV^cand predicted position of U AV^uc for τu= 1 , τd= 1; Figure 3.9 shows the position of U AV^cand predicted position of U AV^uc for τu= 1 , τd= 1; Figure 3.7 shows the position of U AV^c and predicted position of U AV^ucforτu= 2 , τd= 2; Figure 3.8 shows the position of U AV^cand predicted position of U AV^uc forτu= 3 , τd= 1. As it is demonstrated, the small point which always moved

CHAPTER 3. APPLICATION

behind the big red circle is represented to be the position of U AV^c, the big red circle is represented to be the predicted position of U AV^uc. When the U AV^cpoint will "touch"

the U AV^c’s state set, the Algorithm 4 will detect and stop moving the controlled U AV^c, as we can see "stop" from the arrow.

Under the same tracking trajectory and fixed time horizon, we observe that more communication delay the system has, the less accurate state estimation will be, mean- ing that the size of uncontrolled U AV^uc’s reachable state set becomes larger and the controlled U AV^c needs to move more slowly to guarantee the safety.

Figure 3.6: Tracking problem 1 (delayed timeτu= 1 , τd= 1 )

28

CHAPTER 3. APPLICATION

Figure 3.8: Tracking problem 3 (delayed timeτu= 3 , τd= 1 )

30

3.3. SIMULATION

Figure 3.9: Tracking problem 4 (delayed timeτu= 1 , τd= 1 )

C

4

C ONCLUSION AND F UTURE D EVELOPMENT

In this work, we started the state prediction problem under delayed state measurement or delayed output measurement, we developed algorithms for the state prediction problem using reachability of target sets and target tubes [4]. These algorithms and linear MPC theory were used to perform collision avoidance and trajectory tracking tasks for UAVs under communication delay between sensor and controller as well as controller and actuator. In the end, simulations of the two applications were presented, for different communication delays and time steps using a UAV model with 6 degrees of freedom.

The future work could be improving the model with more complex disturbance or improving the complexity of the control system, and performing teleoperation of UAVs over internet.

B IBLIOGRAPHY

[1] M. Behrendt, “A basic working principle of model predictive control,” 2 October 2009. [Online]. Available: https://commons.wikimedia.org/wiki/File:

MPC_scheme_basic.svg

[2] M. Johansson, Linear quadratic and model predictive control. KTH Royal Institute of Technology, 2018.

[3] J. Brown, “What is a drone: Main features & applications of today’s drones.”

[Online]. Available: http://mydronelab.com/blog/what-is-a-drone.html

[4] D.P.Bertsekas and I.B.Rhodes, “On the minimax reachability of target sets and target tubes,” Automatica, vol. VOL. 7, pp. 233–247, 1971.

[5] O. Bokanowski, A. Désilles, and H. Zidani, “Hjb approach for motion planning and reachabilty analysis.” Paris, France: Proceedings of the 5th International ICST Conference on Performance Evaluation Methodologies and Tools, May 16 - 20, 2011.

[6] A. A. Kurzhanskiy and P. Varaiya, “Ellipsoidal techniques for reachability analysis of discrete-time linear systems,” IEEE Transactions on Automatic Control, vol.

VOL.52, p. NO. 1, JANUARY 2007.

[7] A. A. Kurzhanskiy and ravin Varaiya, “Theory and computational techniques for analysis of discrete time control systems with disturbances,” Optimization Meth- ods and Software, vol. VOL. 26:4-5, pp. 719–746, 2011.

[8] A. A. Kurzhanskiy and P. Varaiya, “Ellipsoid tool eecs,” May 6, 2006. [Online].

Available: http://www.eecs.berkeley.edu/Pubs/TechRpts/2006/EECS-2006-46.

html

[9] J. Dohyun, Y. Jaehyun, S. C. Youngdong, K. H. Jin, and J. K. H., “Networked operation of a uav using gaussian process-based delay compensation and model

BIBLIOGRAPHY

predictive control.” Palais des congres de Montreal, Montreal, Canada: 2019 International Conference on Robotics and Automation (ICRA), May 20-24, 2019.

[10] M. Kamel, T. Stastny, K. Alexis, and R. Siegwart, “Model predictive control for trajectory tracking of unmanned aerial vehicles using robot operating system,in Robot Operating System(ROS).” Springer, 2017, pp. 3–39.

[11] J. Zhang, X. Zhou, and Z. Zhou, “Design of time delayed control systems in uav using model based predictive algorithm.” 2nd International Asia Conference on Informatics in Control,Automation and Robotics, 2010.

[12] Wikipedia, “Model predictive control.” [Online]. Available: https://en.wikipedia.org/

wiki/Model_predictive_control#cite_note-6

[13] J. Lofberg, “Yalmip : a toolbox for modeling and optimization in matlab.” New Orleans, LA, USA: 2004 IEEE International Conference on Robotics and Au- tomation (IEEE Cat. No.04CH37508), 2-4 Sept. 2004.

[14] M. Kvasnica, P. Grieder, M. Baotic, and M. Morari, “Multi-parametric toolbox (mpt).”

ETHZ, Zurich: Automatic Control Laboratory, 2003.

[15] F. SABATINO, Quadrotor control: modeling, nonlinear control design, and simula- tion. Stockholm, Sweden: KTH Royal Institute of Technology, June 2015.

[16] L. Asselborn, “Control of stochastic hybrid systems based on probabilistic reachable set computation.” Faculty of Electrical Engineering / Computer Science of the University of Kassel, 2018.

36

APPENDIX

A

A PPENDIX

A.1 Computation for Hyperplanes

In Algorithm 5 with MPC and state estimation, we need to compute hyperplanes of reachable state sets in order to get the safety constraints in MPC (we require that the controlled UAV should be "above" the uncontrolled UAV). As the examples illustrated in Figure A.2, there are many kinds of hyperplanes we can design in order to be the safety constraint. Now we will take one example in two dimension to demonstrate how to compute the coordinates of hyperplane.

Definition A.1. [8] Ellipsoid²(q,Q) in Rⁿwith center q and shape matrix Q is the set

²(q,Q) = {x ∈ Rⁿ| < (x − q),Q⁻¹(x − q) >≤ 1}

where Q is positive definite (Q = Q^T and < x,Qx >> 0 for all nonzero x ∈ Rⁿ). Here < .,. >

denotes inner product.

According to [16], the location of an ellipsoid is defined by its center q, the shape and direction of an ellipsoid is defined by its shape matrix Q. The semi-major axis is the longest radius of an ellipsoid, the length isp

λ1 whereλ1 is the maximum eigenvalue of the shape matrix Q, and direction is defined by the corresponding eigenvector v₁. The semi-minor axis is the shortest radius of an ellipsoid, the length isp

λ2 whereλ2is the minimum eigenvalue of the shape matrix Q, and direction is defined by the corresponding eigenvector v₂.

APPENDIX A. APPENDIX

Figure A.1: Notation of an ellipsoid

Figure A.1 shows the specification of an ellipsoid as we described above.

Hence, for different hyperplanes described in Figure A.2, we all can calculate their hyperplanes’ coordinates according to the eigenvalues and eigenvectors of the shape matrix Q.

Figure A.2: Hyperplane examples

38

TRITA-EECS-EX-2020:34