Optimal control of AUVs

(1)

F

ACULDADE DE

E

NGENHARIA DA

U

NIVERSIDADE DO

P

ORTO

Optimal control applied to AUVs

Joana Filipa Gouveia Fonseca

Mestrado Integrado em Engenharia Eletrotécnica e de Computadores Supervisors: Aníbal Castilho Coimbra de Matos

Maria do Rosário Marques Fernandes Teixeira de Pinho

(2)

c

(3)

Resumo

Na presente tese, determinamos trajetórias verticais de tempo mínimo de um veículo autónomo submarino, ou AUV, usando um modelo dinâmico do veículo subaquático autónomo Mares de-senvolvido em [3]. O problema de controlo ótimo de tempo mínimo é resolvido numericamente usando o método direto: primeiro é discretizado e o problema de programação não-linear tran-scrito é resolvido usando o Interior Point OPTimizer, ou IPOPT (ver [16]). A solução é então parcialmente validada usando o Princípio do Máximo (ver [4]) e é comparada a outras trajetórias possíveis para analisar a sua otimalidade. A solução do nosso problema original é também com-parada com a trajetória de tempo mínimo obtida usando um modelo simplificado proposto em [5].

Uma descoberta notável do nosso trabalho é o facto das trajetórias obtidas com ambos os modelos serem uma linha ondulada que une a posição inicial e o conjunto de posição final. Aqui discutimos esse fenómeno e propomos algumas soluções sub-ótimas. Além disso, estudamos como o tempo mínimo para o nosso problema original muda em função do conjunto de posições finais, fazendo também diversas experiências interessantes com o nosso modelo.

(4)

(5)

Abstract

We determine minimum time vertical trajectories of an AUV using a dynamical model of a Mares autonomous underwater vehicle developed in [3]. The minimum time optimal control problem of interest is solved numerically using the direct method: it is first discretized and the transcribed nonlinear programming problem is then solved using IPOPT (see [16]). The solution is then partially validated using the Maximum Principle (see [4]) and it is compared to other possible trajectories to assert its optimality. The solution of our original problem is also compared to the minimum time trajectory obtained using a simplified model proposed in [5].

A remarkable finding of our work is the fact that the trajectories obtained with both models are a wobbling line joining the initial position and the target set. Here we discus this phenomenon and propose some sub-optimal solutions. Additionally, we study how the minimum time for our original problem changes as a function of the target set and do some other interesting experiments with our model as well.

(6)

(7)

Acknowledgements

I would like to thank my supervisors Maria do Rosário de Pinho and Aníbal Matos for all the effort in teaching, correcting, directing me to the right path and providing all the tools I needed to do this thesis, as well as for the kindness and caring I was lucky enough to get. Also, I want to thank my work colleagues such as Tiago Paiva and Diyako Ghaderyan for hints and for helping when I needed.

I must as well thank my father for his support, care, love and inspiring words to proceed through all the difficulties in life, in my masters and in my thesis.

Finally, I thank the support of the project PTDC/EEIAUT/ 2933/2014, TOCCATA, funded by FEDER funds through COMPETE2020 - POCI and by national funds through FCT - Fundacao para a Ciencia e a Tecnologia.

Joana Fonseca

(8)

(9)

“He who conquers himself is the mightiest warrior.”

Confucius

(10)

(11)

List of Figures

1.1 Portuguese maritime territory . . . 2

2.1 AUV Mares . . . 4

2.2 Optimisation solvers . . . 7

2.3 Optimisation solvers . . . 7

4.1 Cartesian referential with initial and final points for the given example . . . 15

4.2 The four thrusters of the AUV . . . 16

4.3 Fixed and moving referentials . . . 16

5.1 State variables u,w,q and θ . . . 26

5.2 Four thrusters . . . 27

5.3 States x and z and physical trajectory . . . 27

5.4 Hamiltonian . . . 29

5.5 Final target and its multipliers . . . 30

5.6 Switching function for each force . . . 30

5.13 Zoom on the trajectory in x,z . . . 36

5.14 Switching functions for each force . . . 36

5.25 Comparison between the computed controls and (5.29) . . . 45

(14)

xii LIST OF FIGURES

6.7 Total velocity as a function of thruster 3 and 4 and thruster 4 as a function of thruster 3 . . . 60

6.14 Reachable to final target for different tf . . . 64

(15)

List of Tables

2.1 Mares characteristics . . . 4

4.1 AUV parameters with their values . . . 18

5.1 Problem’s balance . . . 52

(16)

(17)

Abbreviations

FEUP Faculdade de Engenharia da Universidade do Porto

DEEC Departamento de Engenharia Eletrotécnica e de Computadores PMP Pontryagin Maximum Principle

AMPL A Mathematical Programming Language WORHP We Optimise Really Huge Problems AUV Autonomous Underwater Vehicle IPOPT Interior Point Optimiser

OCP Optimal Control Problem

(18)

(19)

Chapter 1

Introduction

The present thesis is framed within the Mestrado Integrado em Engenharia Eletrotécnica e de Computadores. Its focus is on the optimal control of a real AUV, through its virtual modelling. So, we determine minimum time vertical trajectories for our AUV Mares by constructing dif-ferent dynamical models, solving the minimum time optimal control problem numerically using the direct method and solving the transcribed nonlinear programming problem with IPOPT. The solution is partially validated using the Maximum Principle and it is compared to other possible trajectories to assert its optimality.

1.1 Problem Statement

To respond to the current demand for AUVs we need to develop fast and energy-efficient vehicles that are capable to compete with others on the market. Here we study the problem of trajectory planning in a vertical plane of AUVs using optimal control. We use different Mares AUV dy-namical models to characterise its movement, formulate optimal control problems with different objective functions, characterise solutions to these problems using analytic tools, solve numeri-cally these problems and analyse the optimality of the solutions.

1.2 Goals and motivation

AUVs are on the brink of the underwater navigation market as they are automatically controlled. They can be very fast (minimum time formulations), very energy efficient (minimum energy for-mulations) or even able to transport specific sensors to collect unique data. This qualities are very useful for different applications.

AUVs can perform in a lot of missions are too dangerous for a manned vehicle, sometimes just impossible for large vehicles and other times too costly to have a crew operating an underwater vehicle for a large period of time.

Since Portugal has a very large maritime area (see figure1.1) that oughts to be safe from illegal activities, explored and maintained, AUVs are of particular interest.

(20)

2 Introduction

Figure 1.1: Portuguese maritime territory

1.3 Report’s structure

Beyond the present introduction, this thesis has 6 more chapters. In chapter 2, we present the state of the art and related work. In chapter3, we state the optimal control problems of interest in this thesis. Besides, we also present the Maximum Principle, an important tool in what comes next. In chapter4, present the dynamic model for the AUV Mares. Taking into account that we restrict our study to the movement of AUVs in a vertical plane, we make several modifications to this model. In this way we come up with a detailed model and a simplified model which are then used in the next chapters. In chapter 5, we define the practical problems and solved them numerically, presenting and validating the results. In chapter 6, we analyse the response of the system to different situations as to provide a better understanding of the previous solutions. In chapter 7, we balance the thesis in a very summarised conclusion and add some future work as suggestion for both ourselves and any reader.

(21)

Chapter 2

State of the art

In this chapter we define the basis from which this thesis was brought up. Thus, we present the basic works upon which this thesis is based.

This chapter follows a logic line of precedence starting with the vehicle Mares as our main vehicle, analysing then trajectory planning as as optimal control problem, going through some dis-cretization methods as well as computational tools for optimisation and finishing with the valida-tion of the optimality of the soluvalida-tion by the Pontryagin’s Maximum Principle and a brief summary.

2.1 AUV: Mares

AUVs are robotic systems that are getting more and more attention both for modelling and control. AUVs market growth is related to the general robotics growth as robots acquire a more dominant role in society from security to scientific research, substituting man in risky or impossible tasks.

Some of the most common applications for AUVs are underwater search, ocean research, environmental monitoring, inspection and repairing of underwater structures, transportation, ar-chaeology and military applications, as seen in [3].

In all of these examples there is the need for robust control that allows smooth and fast travelling. AUVs dynamics is strongly nonlinear, thus classical linear control is of no use.

The AUV Mares, as seen in figure2.1, was developed by The Ocean Systems Group (Ocean-SYS) of the Faculty of Engineering of the University of Porto together with IRS-Porto (Robotic Systems Institute - Porto). This AUV is very flexible because of its modular structure as it may be modified to bare various types of sensors and adapted to different missions. These missions may be programmed through the AUVs GUI (Graphical User Interface) by defining given objectives such as velocity, depth, key points, etc. This AUV may also locate itself through acoustic buoys allowing tools like Feedback Control and Model Predictive Control, as in [11].

Some of the most basic yet relevant characteristic of this AUV are presented in table2.1. Since the aim of this thesis is to apply optimal control to design time and energy efficient trajectories for the AUV Mares, we rely on the dynamical model of this AUV as described in [3]. We present and discuss the dynamical model of the AUV in chapter 4 of this thesis.

(22)

4 State of the art

Figure 2.1: AUV Mares Table 2.1: Mares characteristics

Parameter Value Weight 32Kg Diameter 20cm Length 1.5m Horizontal velocity [0, 2]m/s Autonomy 10h Maximum depth 100m

2.2 Optimal Control and Trajectory Planning

Trajectory planning is a well-known type of control problem in Robotics as it aims at the most basic of needs: the translation of a robot from point A to point B with or without obstacles. There are a variety of ways to do trajectory planning. In this thesis, we intend to contribute to the design of time and energy efficient trajectories in a vertical plane to move the AUV from a initial position to a target set. We address our challenge using optimal control.

Optimal Control concerns the properties of control functions that, when inserted into a dif-ferential equation, give solutions which minimise a given cost. The difdif-ferential equation describes the dynamics of a control system: in our case, an AUV system.

Having applications on the most diverse areas, optimal control problems comprise: dynamic model of the system to be controlled, desired performance index, initial and final state constraints as well as control constraints and, possibly, state constraints.

A very generic optimal control problem is constructed as follows.

The free time optimal control problem we are interested in is to determine (tf, x, u), where

tf > 0, x : [0, +∞[−→ Rnis an absolutely continuous function and u : [0, +∞[−→ Rmis a

measur-able function (see definitions in appendix), minimising a certain cost and satisfying the following conditions:

(23)

2.2 Optimal Control and Trajectory Planning 5

1. The differential equation that defines dynamically the vehicle

˙x(t) = f (t, x(t), u(t)) a.e. t∈ [0,tf]

2. The control constraint that limits the actuation on the system

u(t) ∈ U a.e. t∈ [0,tf]

3. The initial state constraint that defines the starting point

x(0) = x0∈ Rn.

Here x0is fixed.

4. The final state constraint

x(tf) ∈ T, (2.1)

where T ⊂ Rnis closed and bounded set. 5. The state constraints

h(x) ≤ 0 ∀ t ∈ [0,tf]. (2.2)

As to the objective function to be minimised, it may be written in the Bolza form:

J= g(tf, x(tf)) +

Z t_f

0

L(t, x(t), u(t)) dt, in the Lagrange form:

J=

Z t_f

0

L(t, x(t), u(t)) dt, or in the Mayer form:

J= g(tf, x(tf)) Above we have g : [0, +∞[×Rn−→ R, L : [0, +∞[×Rn ×Rm −→ R, f : [0, +∞[×Rn ×Rm_−→ Rn, U ⊂ Rm, T ⊂ Rn, h : Rn−→ R.

Optimal Control applied to trajectory planning has been used in a variety of applications such as ours: the AUV. In fact, among some of the projects in the field, some of the most relevant and related to these work are [9], [6] and [8].

In the literature, trajectory planning of AUVs is done using detailed and simplified models. The approach can be roughly described in the following way.

1 The optimal control is defined using dynamic models for AUV

2 This problem is solved numerically and the computed solutions are analysed using optimal-ity conditions, specially the Maximum Principle.

(24)

6 State of the art

3 To close the gap between the theory and the implementation on the vehicles, several changes on the models, concerning the nature of the controls, are tested using simulations.

4 Issues pretaining robustness of the solution and sensitivity analysis are further investigated. Our work concentrates on 1, 2 and 3 above and it has roots on the work of Monique Chyba and collaborators.

The study of minimum time problems for AUV is apparently first proposed in [5], where, the structure of time optimal trajectories is investigated. The numerical study of the minimum time problem is then performed in [8]: the movement of the AUV is constrained to a vertical space where the initial and the final position are at the same depth. A journey taking us from theory to implementation for time efficient trajectories appears in [7] and proceeds in [14]. The same approach for minimum energy problems are also studied in [13]. It’s important to note that both [13] and [14] use the dynamical model of a particular vehicle, the ODIN.

In [6], the trajectory planning of a vehicle in the horizontal plane is studied following the same approach but using a AUV simplified model. An interesting aspect of [6] is the fact that ocean currents are taken into account.

2.3 Discretization methods

Solving any optimal control problem requires discretization so as to computationally optimise finite length vectors of data - the controls and the states. As in the Chapter 4 of [10], some discretization methods like The Euler Method are presented. These methods will be needed for the discretization of the problem along time for the optimisation solver.

For a differential equation of the form ˙z(t) = f (t, z(t)), some of the most simple and useful methods are:

The explicit Euler method:

zN(ti+1) := zN(ti) + hif(ti, zN(ti)) (2.3)

The implicit Euler method:

zN(ti+1) := zN(ti) + hif(ti+1, zN(ti+1)) (2.4)

The Taylor order 2 method:

zN(ti+1) := zN(ti) + hif(ti+1, zN(ti+1)) +

h2_i 2 f

0_(t

i+1, zN(ti+1)) (2.5)

And many others. For our free time problems optimal control problems, the two first methods applied ˙x(t) lead to:

x[i + 1] = x[i] +tf

(25)

2.4 Computational tools for optimisation 7

x[i + 1] = x[i] + 0.5tf

n( ˙x[i] + ˙x[i + 1]) (2.7)

2.4 Computational tools for optimisation

The numeric resolution of optimal control problems is done either with the Direct Method or the Indirect Method. For this thesis, we’ll be using the diret method only. This method consists on the discretization of the optimal control problem, making it a non linear programming problem of great dimension. This problem is then solved using optimisation solvers. These are, on the majority, available for free in the Internet.

Various solvers exist (see figure2.2), being the most commonly referred in literature for optimal control, KNITRO and IPOPT.

Note that WHORP is a recent tool and may soon take the lead above IPOPT (see figure2.3).

Figure 2.2: Optimisation solvers

Figure 2.3: Optimisation solvers

The utilisation of some of these solvers requires an interface. There are several known, one may be MATLAB. For optimal control, there is ICLOCS (Imperial College of London Optimal Control Solver) that runs on MATLAB. Another very widely used is AMPL (A Mathematical Programming Language).

The difference between these two consists on the following: with ICLOCS we can write di-rectly the dynamical equations and the program chooses the adequate discretization, while AMPL requires the programmer to introduce directly the already discretized dynamic system, leaving the choice of the method of discretization to the programmer.

(26)

8 State of the art

2.5 Optimality validation: Pontryagin’s Maximum Principle

As stated in [12], "The Maximum Principle provides a set of necessary conditions which should be satisfied by any optimal control problem. Not surprisingly the idea behind derivation of necessary conditions in the form of Maximum Principle is to obtain MPs that produces the smallest set of candidates to the optimal control problems. It is well known that for some problems the classical Maximum Principle is not only a necessary condition of optimality but also a sufficient condition."

The free time general optimal control problem of interest is defined as:

(P)                            Minimise tf+ l(x(tf)) s.t. ˙x(t) = f (x(t), u(t)) a.e. t∈ [0,tf], u(t) ∈ U a.e. t∈ [0,tf] x(0) = x0 x(tf) ∈ T (2.8)

We consider the functions g and f to be continuously differentiable with respect to the states x and the controls u, u to be measurable and U and T to be closed convex sets. The Hamiltonian is defined as:

Hλ_{(x, p, u) = hp, f (x, u)i}

The Pontryagin’s Maximum Principle has the following statement, as in [12] and [4]: (¯x, ¯u) is a strong local minimiser for problem (P). Then there exist an arc p ∈ W1,1([0,tf]; Rn) (see

definition in appendixA) and a scalar λ ≥ 0 such that the following conditions are satisfied: (i) The Non triviality Condition

(p, λ ) 6= (0, 0) (ii) The Euler Adjoint Equation

− ˙p(t) = Hλ

x(¯x(t), p(t), ¯u(t)) a.e. t∈ [0,tf]

(iii) The Transversality Condition

−p(tf) ∈ λ 5xl( ¯x(t f )) + NT( ¯x(t f ))

(iv) The Weierstrass Condition Hλ_{(¯x(t), p(t), ¯u(t)) = max}

u∈UH

λ₍_{¯x(t), p(t), u)} _a.e. _t_{∈ [0,t} f];

(v) The constancy condition

Hλ₍_{¯x(t), p(t), ¯u(t)) = λ} _a.e. _t_{∈ [0,t} f];

(27)

Chapter 3

Optimal control

In the previous chapter we introduce the general formulation of the optimal control problem. In the forthcoming chapters of this thesis we’ll present several problem formulations, however, only some will be validated. Out of those, we only need to write the maximum principle for the minimum time and the minimum energy OCP. Starting with the generic formulation of the OCP from the previous chapter, we make some simplifications to adapt it to our problem, as we now know the objective function will either be the final time or a the state y(tf). Also, we will rewrite

the MP for an OCP with controls appearing linearly on the model. We can do so because in our AUV model (see chapter4) the control appears linearly.

We mainly consider here (like in the rest of the thesis) minimum time and minimum energy problems. Those, we describe below in section 1 and 2. Keeping in mind that our aim is to solve numerically the OCP, the question is how we can verify or refute the solution obtained. The Pontryagin’s Maximum Principlestates conditions that an optimal solution should satisfy. If the candidate to the solution of the OCP tests these conditions positively it does not follow that it is a solution. After all, the PMP is only a necessary condition for optimality.

For the dynamical model we refer the reader to chapter4. Note that we concentrate on a problem where the dynamics is of the form:

     ˙x1(t) = f1(x1(t)) + 4

∑

i=1 biui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t))

Thus, from chapter2we get that x = [x1, x2] and f (x, u) = [ f1(x1) + 4

∑

i=1

biui, f2(x1)].

Sections 1 and 2 also include the statement of the PMP for both the minimum time and the minimum energy problems.

(28)

10 Optimal control

3.1 Minimum Time Problem

The minimum time problem is defined as follows:

(M.T.P)                                    Minimise tf s.t. ˙x1(t) = f1(x1(t)) + 4

∑

i=1 biui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t)) u(t) ∈ U x(0) = x0 x(tf) ∈ T (3.1) Hλ_{(x, p, u) = hp} 1, f1(x1) + 4

∑

i=1 biuii + hp2, f2(x1)i (3.2)

Although f1and f2can be nonlinear, the control appears linearly in the differential equation.

Throughout this chapter we consider f1 and f2to be continuously differentiable and the set T to

be convex and closed. Besides, we set U ⊂ R4 and U = [u1min, u1max] × ... × [u4min, u4max]. Now

we proceed to state the necessary conditions for the OCP, as stated in section2.5.

Let (tf, ¯x, ¯u) be a local minimiser of OCP (see definition in appendixA). Then, there exists

an absolutely continuous function (see definition in appendix Aas well) p : [0,tf] −→ Rn and a

scalar λ ≥ 0:

(i) (p,λ ) 6= (0,0)

(ii) - ˙p(t) = 5xHλ(¯x(t), p(t), ¯u(t)) a.e. t ∈ [0,tf]

(iii) -p(tf) ∈ NT(¯x( ¯tf))

(iv) Hλ_{(¯x(t), p(t), ¯u(t)) = max} u∈UH

λ_{(¯x(t), p(t),u)} _{a.e. t ∈ [0,t} f] ;

(v) Hλ_{(¯x(t), p(t), ¯u(t)) = λ} _{a.e. t ∈ [0,t} f] ;

Condition (i) above is a non-triviality condition. It states that if p and λ are both zero, than any admissible solution of the OCP would satisfy conditions (ii)-(iv). So, the (M.T.P)’s solution wouldn’t be relevant.

As for (ii), it is the so called Euler Lagrange equation. Since Hλ_{(x, p, u) = hp}

1(t), f1(x(t)) + 4

∑

i=1

biuii + hp2, f2(x)i, (ii) reads:

− ˙p(t) = hp1, f1x(x)i + hp2, f2x(x)i = f

T

(29)

3.1 Minimum Time Problem 11

Condition (iii) is called the transversality condition and it states that the value of the adjoint mul-tiplier p at tf, times −1, should belong to the normal cone NT(¯x( ¯tf)) of the set T at the end state

¯x(tf). If ¯x(tf) ∈ intT , then NT(¯x(tf)) = {0}. Since T is convex,

NT(¯x(tf)) =

ξ ∈ RN: hξ , x − ¯xi ≤ 0, ∀x ∈ T .

Condition (iv) is the Weierstrass condition. Since Hλ_{(x, p, u) = hp}

1, f1(x) + 4

∑

i=1

biui)i +

hp₂, f2(x)i, we have from (iv):

hp1(t), f1(¯x(t)) + 4

∑

i=1

biu¯i(t))i + hp2(t), f2(¯x(t))i = max

u∈Uhp1(t), f1(¯x(t)) + 4

∑

i=1 biui)i + hp2(t), f2(¯x(t))i m hp₁(t), 4

∑

i=1 biu¯i(t)i = max u∈Uhp1(t), 4

∑

i=1 biuii a.e. t ∈ [0,tf]; Set Y(u) = hp1, 4

∑

i=1

biuii. Since Y is linear, maxu∈UY(u) should be maximum at umin, umax

or at the point in which 5Y ( ¯u) = 0. If u ∈ R, then hp1, bii is called the switching function of ui.

We then conclude that, for i=1,2,3,4, the optimal control is defined as:

¯ ui(t) =          uimax if p T 1(t)bi> 0 ]uimin, uimax[ if p T 1(t)bi= 0 uimin if p T 1(t)bi< 0

Condition (v) is called the constancy condition. The Hamiltonian makes its entrance here because this is a free time problem. Observe that if λ = 1 (i.e., the problem is normal) then, the Hamiltonian along the optimal solution is constant and equal to 1.

(30)

12 Optimal control

3.2 Minimum Time Energy Problem

Our aim now is to minimise the time and the energy to drive the system from a given point x0

to a target T. This means that now we need to change the cost function in OCP. This leads to the following problem: (M.E.P)                                        Minimise Z t_f 0 1 + 4

∑

i=1 αiu2i(t) ! dt s.t. ˙x1(t) = f1(x1(t)) + 4

∑

i=1 biui(t) ˙x2(t) = f2(x1(t)) u(t) ∈ U x(0) = x₀ x(tf) ∈ T (3.4)

This problem is not in the Mayer form (as in the previous chapter, optimal control section). To reformulate it in the Mayer form, we apply the so called state augmentation technique: we add an extra state y. ˙ y= 1 + 4

∑

i=1 αiu2i(t) y(0) = 0 This yields the following problem:

(M.E.P)                                                    Minimise y(tf), s.t. ˙y(t) = 1 + 4

∑

i=1 αiu2i(t) a.e. t ∈ [0,tf], ˙x1(t) = f1(x1(t)) + 4

∑

i=1 biui(t) ˙x2(t) = f2(x1(t)) u(t) ∈ U x(0) = x0 y(0) = 0 x(tf) ∈ T (3.5) H(x, p, u, λ ) = hp1, f1(x) + 4

∑

i=1 biuii + hp2, f2(x)i − λ (1 + 4

∑

i=1 αiu2i) (3.6)

(31)

3.2 Minimum Time Energy Problem 13

Observe that the pair (x,y) is now our state variable. It follows that the control does not appear linearly. Then, applying the PMP to this new problem, we get:

(i) (p,λ ) 6= (0,0)

(ii) - ˙p(t) = 5xH(¯x(t), p(t), ¯u(t), λ ) a.e. t ∈ [0,tf]

(iii) -p(tf)∈ NT(¯x(tf))

(iv) H(¯x(t), p(t), ¯u(t),λ ) = max

u∈U H(¯x(t), p(t), u(t), λ ) a.e. t ∈ [0,tf]

(v) H(¯x(t), p(t), ¯u(t),λ ) = 0 a.e. t∈ [0,tf]

By (3.6), condition (iv), reads as follows:

hp₁(t), f1(¯x(t)) + 4

∑

i=1 biu¯i(t)i + hp2(t), f2(¯x(t))i + hp3(t), 1 + 4

∑

i=1 αiu2i(t)i = max u∈Uhp1(t), f1(¯x(t)) + 4

∑

i=1 biui)i + hp2(t), f2(¯x(t))i + hp3(t), 1 + 4

∑

i=1 αiu¯2ii m hp₁(t), 4

∑

i=1 biui(t)i − λ 4

∑

i=1 αiu2i(t) = max u∈U{hp1(t), 4

∑

i=1 biuii − λ 4

∑

i=1 αiu2i} a.e. t ∈ [0,tf] Set Y(u) = hp1, 4

∑

i=1

biuii − λ

∑

αiui2. Since Y is quadratic, max

u∈U Y(u) should be maximum

at uimin, uimaxor at the point in which 5Y ( ¯u) = 0.

From ∂Y ( ¯u) ∂ ui = pT₁bi− 2λiui= 0, we can get: ¯ ui(t) = pT₁(t)bi 2λ αi (3.7) Then, from the above equation we get, for i = 1, .., m:

¯ ui(t) =            uimax if pT 1(t)bi 2λ αi > uimax (pT 1(t)bi 2λ αi if uimin < pT 1(t)bi 2λ αi < uimax uimin if pT₁(t)bi 2λ αi < uimin

Therefore, we have a closed formula for this optimal control.

¯ ui(t) = max uimin, min (pT 1(t)bi 2λ αi , uimax .

(32)

(33)

Chapter 4

Modelling

In this chapter, we briefly describe the dynamical model of the Mares AUV. We refer the reader to [3] for a complete description.

This project lies on the optimisation of trajectories of a submarine for minimum time or minimum energy, with varying constraints. This means that we intend, for a given mission, to optimise according to the objectives, the trajectory of the submarine. We aim at calculating the trajectories the vehicles should follow starting at a given point (x0, z0) and finishing at another

point (x1, z1). We consider that the movement is along the vertical plane xz, as presented in the

following figure4.1.

Figure 4.1: Cartesian referential with initial and final points for the given example

This means that, in this project, the dynamical model of the AUV will not consider the rota-tion (roll), the lateral movement (sway) or the rotarota-tion in the vertical axis (yaw). This corresponds to considering ˙φ = p = 0, ˙y= v = 0, ˙ψ = r = 0.

Given this, the original and generic model of the AUV with 6 degrees of freedom, turns into a simplified model with 3 degrees of freedom.

The variables of the dynamical models are as follows.

• The vector u = [u1, u2, u3, u4] represents the propulsive forces that control the system (see

figure4.2).

(34)

16 Modelling

- The forces u1 and u2 are in the x axis of the AUV and since the movement, in our

case, will only occur in the vertical plane, u1=u2. Note that, as in figure4.2, u1and u2are

at the same position in x and z, just different position y. Thus, if u1is different from u2, a

movement in the plane xy will occur.

- The forces u3and u4are in the z axis of the AUV, being the u4at the front and u3at

the rear, as in4.2.

Figure 4.2: The four thrusters of the AUV

• The vector x = [u, w, q, θ , x, z] is the state vector of the system. It can be rewritten as x = [x1, x2], with x1= [u, w, q, θ ] and x2= [x, z]. Here:

- u is the velocity in the axis x of the AUV referential, as in4.3. - w is the velocity in the axis z of the AUV referential, as in4.3. - q is the angular velocity of the angle θ .

- θ is the pitch angle, the angle made between the AUV referential and the fixed refer-ential, as in4.3.

- x is a coordinate of the position in the fixed referential, as in4.3. - z is a coordinate of the position in the fixed referential, as in4.3.

Figure 4.3: Fixed and moving referentials

The original dynamical equations as obtained in [3] are:

(m − Xu˙) ˙u− Xq˙q˙= mrv − mqw + (Zw˙w+ Zq˙q)q − (Yv˙v+Yp˙p+Y˙rr)r

+Xu|u|u |u|+ Xq|q|q |q|− (W − B)sin(θ ) + u1+ u2

(35)

4.1 Detailed model 17

(m −Yv˙) ˙v−Yp˙p˙−Y˙r˙r = −mru + mpw − (Zw˙w+ Zq˙q)p + (Xu˙u+ Xq˙q)r

+Yv|v|v |v|+Yp|p|p |p|+Yr|r|r |r| (4.2) (m − Zw˙) ˙w− Zq˙q˙= mqu − mpv + (Yv˙v+Yp˙p+Y˙rr)p − (Xu˙u+ Xq˙q)q +Zw|w|w |w|+ Zq|q|q |q|− (B −W )cos(θ ) + u3+ u4 (4.3) (Ixx− Kp˙) ˙p− Kv˙v˙= −Ixxrq+ Iyyqr+ (Zw˙w+ Zq˙q)v − (Yv˙v+Yp˙p+Y˙r)w+ (Y˙rv+ N˙rr)q − (Xq˙u+ Zq˙w+ Mq˙q)r + Kp|p|p |p|+ Kv|v|v |v| (4.4) (Iyy− Mq˙) ˙q− Mu˙u˙− Mw˙w˙= Izr p− Ixxpr− (Zw˙w+ Zq˙q)u + (Xu˙u+ Xq˙q)w

−(Y_˙rv+ N˙rr)p + (Yp˙v+ Kp˙p)r + Mq|q|q |q|+ Mw|w|w |w|+ Mu|u|u |u|

−zc fBsin(θ ) + zp1u1+ zp2u2− xp3u3− xp4u4 (4.5) (Izz− N˙r)˙r − Nv˙v˙= −Iyyqp+ Ixxpq+ (Yv˙v+Yp˙p+Y˙r)u − (Xu˙u+ Xq˙q)v +(Xq˙u+ Zq˙w+ Mq˙q)p − (Yp˙v+ Kp˙p)q +Nr|r|r |r|+ Nv|v|v |v|− yp1u1− yp2u2 (4.6)

Some of the parameters above are in table4.1.

4.1 Detailed model

Since the motion is within the vertical plane and equations (4.1) to (4.6) are for the 3D space, we can do some simplifications. Though we can remove any information that regards change in the y-axis, linear or angular, of position, velocity or acceleration: the rotation in roll (p = ˙p= 0), the rotation pitch (r = ˙r = 0) and the movement in the y axis (v = ˙v= 0). Then, we get:

(m − Xu˙) ˙u− Xq˙q˙= −mqw + (Zw˙w+ Zq˙q)q + Xu|u|u |u|+ Xq|q|q |q| −(W − B)sin(θ ) + u1+ u2 (4.7) (m − Zw˙) ˙w− Zq˙q˙= mqu − (Xu˙u+ Xq˙q)q + Zw|w|w |w|+ Zq|q|q |q| −(B −W )cos(θ ) + u3+ u4 (4.8) (Iyy− Mq˙) ˙q− Mu˙u˙− Mw˙w˙ = −(Zw˙w+ Zq˙q)u + (Xu˙u+ Xq˙q)w +Mq|q|q |q|+ Mw|w|w |w|+ Mu|u|u |u| −zc fBsin(θ ) + zp1u1+ zp2u2− xp3u3− xp4u4 (4.9)

(36)

18 Modelling

Table 4.1: AUV parameters with their values

Parameter Description Value

W Weight of the vehicle 314

B Fluctuation force 316

m Mass of the vehicle 32

Iyy Moment of inertia along y 4.73

Xu|u| Drag force along axis x due to velocity u −4.05

Xq|q| Drag force along axis x due to velocity q −0.0484

Zw|w| Drag force along axis z due to velocity w −116

Zq|q| Drag force along axis z due to velocity q −5.95

Mu|u| Drag force along rotation θ due to velocity u −0.211

M_q|q| Drag force along rotation θ due to velocity q −15.6 Mw|w| Drag force along rotation θ due to velocity w −8.26

Xu_˙ Influence of acceleration ˙ufor the movement along axis x −1.74

Xq˙ Influence of acceleration ˙qfor the movement along axis x −0.0305

Zw˙ Influence of acceleration ˙wfor the movement along axis z −41.2

Zq_˙ Influence of acceleration ˙qfor the movement along axis z −0.123

Mu˙ Influence of acceleration ˙ufor the rotation along θ −0.035

Mq˙ Influence of acceleration ˙qfor the rotation along θ −6.07

Mw_˙ Influence of acceleration ˙wfor the rotation along θ −0.123

zc f Height of the fluctuation center wrt the gravity center −0.0044

zp1 Coordinate in which the force from thruster 1 is applied −0.0012

z_p2 Coordinate in which the force from thruster 2 is applied −0.0012 zp3 Coordinate in which the force from thruster 3 is applied −0.421

zp4 Coordinate in which the force from thruster 4 is applied 0.434

These equations can be presented in the form of matrices, as follows.       m− Xu_˙ 0 −Xq˙ 0 0 m− Zw˙ −Zq˙ 0 −Mu_˙ −Mw˙ Iyy− Mq˙ 0 0 0 0 1             ˙ u ˙ w ˙ q ˙ θ       = (4.10)      

−mqw + (Zw˙w+ Zq˙q)q + Xu|u|u |u|+ Xq|q|q |q|− (W − B)sin(θ )

mqu− (Xu_˙u+ Xq˙q)q + Zw|w|w |w|+ Zq|q|q |q|− (B −W )cos(θ )

−(Zw_˙w+ Zq˙q)u + (Xu˙u+ Xq˙q)w + Mq|q|q |q|+ Mw|w|w |w|+ Mu|u|u |u|− zc fBsin(θ )

q       + (4.11)       u1+ u2 u₃+ u4 zp1u1+ zp2u2− xp3u3− xp4u4 0       (4.12)

(37)

4.1 Detailed model 19

The matrices on4.10represent the independent left part of the equalities, the matrix on4.11

represents the state dependant right part of the equalities and the matrix on4.12 represents the control dependent right part of the equalities.

Next, the equations with regard to x1= [u, w, q, θ ] and x2= [x, z] can be written into ˙x1=

M( f (x1) +Cu) and ˙x2= f2(x1), as       ˙ u ˙ w ˙ q ˙ θ       = M ×       f(u, w, q, θ ) +C       u₁ u2 u3 u₄             , " ˙ x ˙z # = f2(u, w, q, θ ) with: f(u, w, q, θ ) =      

−mqw + (Zw_˙w+ Zq˙q)q + Xu|u|u |u|+ Xq|q|q |q|− (W − B)sin(θ )

mqu− (Xu˙u+ Xq˙q)q + Zw|w|w |w|+ Zq|q|q |q|− (B −W )cos(θ )

q       (4.13) where: M= 1 K×       (m − Zw˙)(Iyy− Mq˙) − Zq˙Mw˙ Xq˙Mw˙ Xq˙(m − Zw˙) 0 Zq˙Mu˙ (m − Xu˙)(Iyy− Mq˙) − Xq˙Mu˙ Zq˙(m − Xu˙) 0 Mu_˙(m − Zw˙) Mw˙(m − Xu˙) (m − Xu˙)(m − Zw˙) 0 0 0 0 K       K= (m − Xu˙)(m − Zw˙)(Iyy− Mq˙) − Xq˙Mu˙(m − Zw˙) − Zq˙Mw˙(m − Xu˙) C=       1 1 0 0 0 0 1 1 z_p1 z_p2 −x_p3 −x_p4 0 0 0 0       f2(u, w, q, θ ) = " cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w # (4.14)

This model may further be represented as:

˙x1= f1(x1) + 4

∑

i=1 biui , ˙x2= f2(x1) where f₁= M f (x1) , B= MC

(38)

20 Modelling

4.2 Simplified model

For the simplified model, we are going to make some acceptable simplifications by making some of the previous parameters equivalent to zero. We bring up this simplified model to build the bridge between the current thesis and the paper [8].

We assume that the difference between the weight and the fluctuation is zero. Also, we assume all of the drag forces as well as Xq˙, Zq˙, Mu˙and Mw˙ are all zero as to uncouple the distinct

movements. The resulting equations are:

(m − Xu˙) ˙u= −mqw + Zw˙wq+ u1+ u2 (4.15) (m − Zw˙) ˙w= mqu − Xu˙uq+ u3+ u4 (4.16) (Iyy− Mq˙) ˙q= −Zw˙wu+ Xu˙uw+ zp1u1+ zp2u2− xp3u3− xp4u4 (4.17) In matrix form:       ˙ u ˙ w ˙ q ˙ θ       =       1 m−Xu˙ 0 0 0 0 _m−Z1 ˙ w 0 0 0 0 _I 1 yy−Mq˙ 0 0 0 0 1       × (4.18)             −wq(m − Zw˙) uq(m − Xu˙) uw((m − Zw˙) − (m − Xu˙)) q       +       1 1 0 0 0 0 1 1 zp1 zp2 −xp3 −xp4 0 0 0 0             u1 u₂ u3 u4             " ˙ x ˙z # = " cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w # (4.19)

The simplified model proposed in [8] is:            ˙ x ˙z ˙ θ ˙ u ˙ w ˙ q            =            cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w q −wqm3 m1 uqm1 m3 uwm3−m1 I            +            0 0 0 0 0 0 0 0 0 1 m1 0 0 0 _m1 3 0 0 0 1_I               f1 f2 f₃    (4.20)

(39)

4.2 Simplified model 21 Defining m1= m − Xu˙, m3= m − Zw˙, I = Iyy− Mq˙, f1= u1+ u2, f2 = u3+ u4 and f3 = zp1u1+ zp2u2− xp3u3− xp4u4in (4.20) we get:            ˙ x ˙z ˙ θ ˙ u ˙ w ˙ q            =            cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w q −wqm−Zw˙ m−Xu˙ uqm−Xu˙ m−Zw˙ uwXu˙−Zw˙ Iyy−Mq˙            +            0 0 0 0 0 0 0 0 0 1 m−Xu˙ 0 0 0 _m−Z1 ˙ w 0 0 0 _I 1 yy−Mq˙               u₁+ u2 u3+ u4 zp1u1+ zp2u2− xp3u3− xp4u4    (4.21) which coincides with (4.18) and (4.19) We can write (4.21) in the form

˙x = f1(x1) + MCu with x = [x, z, θ , u, w, q], f =            cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w q −wqm−Zw˙ m−Xu˙ uqm−Xu˙ m−Zw˙ uwXu˙−Zw˙ Iyy−Mq˙            , M =            0 0 0 0 0 0 0 0 0 0 0 0 1 m−Xu˙ 0 0 0 0 _m−Z1 ˙ w 0 0 0 0 _I 1 yy−Mq˙ 0            ,C =       1 1 0 0 0 0 1 1 zp1 zp2 −xp3 −xp4 0 0 0 0       (4.22) Again, observe that this system may also be represented as:

˙x1= f1(x1) + 4

∑

i=1 biui , ˙x2= f2(x1) with f₁= f (x1) , B= MC

(40)

(41)

Chapter 5

Numerical Results

In this section we obtain optimal and sub optimal solutions for our problems through numerical simulation using optimisation software. The results contemplate a complete analysis on the be-haviour of our AUV, as well as solution optimality validation analysis. For these problems we use AMPL with the Implicit Euler Method and the solver IPOPT.

5.1 Introduction

Given the model in the previous chapter, we want to determine the trio (x,u,tf) (being x the state

associated with the control u and tf the final time) that satisfies the AUV’s differential equation

and that, at the same time, minimises a given cost (performance index). Recall that x = [x1, x2],

x1= [u, w, q, θ ] and x2= [x, z]

Our problem is then:

1. Minimise the objective function:

minimise ui,tf Z t_f 0 1 + 4

∑

i=1 αiu2i ! dt. (5.1)

2. Satisfy the differential equations of the dynamic model of the AUV having x1, x2, f1(x),

f2and u as previously defined:

˙x1= f1(x1) + 4

∑

i=1

biui , ˙x2= f2(x1)

3. Satisfy the initial state constraints:

x(0) = x0 (5.2)

4. Satisfy the control constraints:

ui ∈ U (5.3)

(42)

24 Numerical Results

5. Satisfy the state constraints:

h(x(t)) ≤ 0 (5.4)

6. Satisfy the final state constraints:

x1(tf) = xf

x2(tf) ∈ B( ˜xf; ε)

(5.5)

where x0∈ R6, xf ∈ R4and ˜xf ∈ R2.

In (5.1), the αkare the parameters. Different choices for these parameters allow us to consider

different problems that differ only in the objective function. Their aim is to distribute the relative weight of each variable in the objective function, according to what we desire for a given problem. For example, for the minimum time problem, we use αi= 0 for i = 1, 2, 3, 4, the objective

function is merely tf. For a minimum energy problem, we may use αi ∈]0, 1] for i = 1, 2, 3, 4.

Another very useful option is to vary each parameter according to our desire.

The constraints (5.2) indicate that the initial state in terms of linear velocity (u and w), angu-lar velocity (q), linear position (x and z) and anguangu-lar position (θ ) of the AUV.

The inclusion (5.3) indicates the actuation limits of the 4 propellers, having ui(t) varying

between the limit values of uimin and uimax.

The boundary constraints (5.5) indicate the circle in which the AUV should lie at tf.

Summarising, we concentrate on the generic problem (P):

(P)                                                minimise g(tf, x(tf)) s.t. ˙x1= f1(x1) + 4

∑

i=1 bi× ui ˙x2= f2(x1) a.e. t ∈ [0,tf], u(t) ∈ U x(0) = x0 h(x(t)) ≤ 0 x1(tf) = xf x2(tf) ∈ B( ˜xf; ε) (5.6) where g : [0, +∞] × R6−→ R, f : R6_{× R}4_{−→ R}6_{, U ⊂ R}4_{, B( ˜}_x f; ε) ⊂ R2.

We assume that g and f are both continuously differentiable functions on their domain. Also we assume U and B( ˜xf; ε) are both compact sets.

Next we treat different problems: the minimum time problem and the minimum time and energy problem using the detailed and the simplified models of the previous chapter. blalabla

(43)

5.2 Minimum time 25

5.2 Minimum time

In the first approach, we use the detailed model of the AUV and we want to minimise the final time. The controllers are restricted within their force limits. The initial and final constraints are intended to be kept identical throughout most of the different problems for further comparison purposes.

Note that f1det refers to the vector f1and bidet refers to the column vectors of matrix B of the

detailed model of chapter4. Also, f2refers to the vector f2of chapter4as well.

We consider the problem:

(M.T)                                                                minimise tf s.t.         ˙ u ˙ w ˙ q ˙ θ         = f1det(u, w, q, θ ) + 4

∑

i=1 bidetui a.e. t ∈ [0,tf], f₂(u, w, q, θ ) =   cos(θ )u + sin(θ )w −sin(θ )u + cos(θ )w   (u1(t), u2(t), u3(t), u4(t)) ∈ [−30, 30] × [−30, 30] × [−30, 30] × [−30, 30] (u(0), w(0), q(0), θ (0), x(0), z(0)) = (0, 0, 0, 0, 0, 0) (u(tf), w(tf), q(tf), θ (tf)) = (0, 0, 0, 0) (x(tf) − 40)2+ (z(tf) − 10)2≤ 0.05 (5.7) Having: f1= M f (u, w, q, θ ) , B= MC with: f(u, w, q, θ ) =      

−mqw + (Zw_˙w+ Zq˙q)q + Xu|u|u |u|+ Xq|q|q |q|− (W − B)sin(θ )

mqu− (Xu˙u+ Xq˙q)q + Zw|w|w |w|+ Zq|q|q |q|− (B −W )cos(θ )

q       (5.8) where: M= 1 K×       (m − Zw˙)(Iyy− Mq˙) − Zq˙Mw˙ Xq˙Mw˙ Xq˙(m − Zw˙) 0 Zq_˙Mu˙ (m − Xu˙)(Iyy− Mq˙) − Xq˙Mu˙ Zq˙(m − Xu˙) 0 Mu˙(m − Zw˙) Mw˙(m − Xu˙) (m − Xu˙)(m − Zw˙) 0 0 0 0 K      

(44)

26 Numerical Results K= (m − Xu˙)(m − Zw˙)(Iyy− Mq˙) − Xq˙Mu˙(m − Zw˙) − Zq˙Mw˙(m − Xu˙) C=       1 1 0 0 0 0 1 1 z_p1 z_p2 −x_p3 −x_p4 0 0 0 0       5.2.1 Results

Here we present the numerical results:

n = 1000, tf = 12.9387, OBJ = 12.9387,

p1(0) = −0.505449, p2(0) = −0.12464, p3(0) = 0.0306387,

p4(0) = −0.137314, p5(0) = −0.251899, p6(0) = −0.0608948.

(5.9)

Figure 5.1: State variables u,w,q and θ

In figure5.1we can get a clear view of the linear and angular velocities as well as the angle. The velocity in x appears to be just as expected beginning with a gradual increase in velocity, followed by a period of time with almost constant velocity and finishing with a sharp braking. However, the velocity in z, the angular velocity and the angle, appear to be unstable varying between negative and positive values as if oscillating.

In figure5.2 we can see the plot of all the four controllers. The rear thrusters 1 and 2 are bang-bang and full-power accelerating for about 11 seconds, then approximately 2 seconds of braking full-power. The upper thruster 3 is find bang then, most of the time, singular and then bang again. The upper thruster 4 is mostly bang-bang with a short singular arc in the end. Both thruster 3 and 4 have an unstable behaviour.

(45)

5.2 Minimum time 27

Figure 5.2: Four thrusters

Figure 5.3: States x and z and physical trajectory

In figure5.3we can confirm what the previous figures suggested: the solution path is wob-bling, oscillating around a straight line. As expected, the path decomposed in x is approximately a straight line and the path decomposed in z contains the wobbling fraction of the movement.

We need to validate this possibly optimal solution with the maximum principle stated in4.

5.2.2 Validation

(46)

28 Numerical Results ∃ (p, λ ) 6= (0, 0) s.t. Hλ_{(¯x(t), p(t), ¯u(t)) = λ} _a.e. _t_{∈ [0,t} f] − ˙p(t) = fxT(¯x(t), ¯u(t))p(t) − p(tf) ∈ NT(¯x(tf)) hp₁(t), 4

∑

i=1

biu¯i(t)i = maxu(t)∈Uhp1(t), 4

∑

i=1

biui(t)i

(5.10)

For our detailed model the adjoint equations are:

˙ p1= −1 Iyy− Mq (Xu− m) (Zw− m) + MuXq(Zw− m) + MwZq(Xu− m) × Zqp2(Xu− m) − p3(Xu− m) (Zw− m) + Xqp1(Zw− m) 2 Muuu− Zqq+ Xuw− Zww − (Xuq− m q) p2 MuXq+ Iyy− Mq (Xu− m) − MwXqp1+ Mwp3(Xu− m) +2 Xuuu p1 MwZq+ Iyy− Mq (Zw− m) − MuZqp2+ Mup3(Zw− m) ˙ p2= −1 Iyy− Mq (Xu− m) (Zw− m) + MuXq(Zw− m) + MwZq(Xu− m) × 2Zww|w| p2 MuXq+ Iyy− Mq (Xu− m) − MwXqp1+ Mwp3(Xu− m) + (Zwq− m q) p1 MwZq+ Iyy− Mq (Zw− m) − MuZqp2+ Mup3(Zw− m) + Zqp2(Xu− m) − p3(Xu− m) (Zw− m) + Xqp1(Zw− m) Xqq+ Xuu− Zwu+ 2Mww|w| ˙ p₃= p4− −1 Iyy− Mq (Xu− m) (Zw− m) + MuXq(Zw− m) + MwZq(Xu− m) × p2 MuXq+ Iyy− Mq (Xu− m) − MwXqp1+ Mwp3(Xu− m) m u− Xuu− 2 Xqq+ 2Zqq|q| + p1 MwZq+ Iyy− Mq (Zw− m) − MuZqp2+ Mup3(Zw− m) 2 Zqq+ Zww− m w + 2Xqq|q| + Zqp2(Xu− m) − p3(Xu− m) (Zw− m) + Xqp1(Zw− m) Xqw− Zqu+ 2Mqq|q| ˙ p4= − −1 Iyy− Mq (Xu− m) (Zw− m) + MuXq(Zw− m) + MwZq(Xu− m) × cos(θ ) (B −W ) p1 MwZq+ Iyy− Mq (Zw− m) − MuZqp2+ Mup3(Zw− m) + sin(θ ) (B −W ) p2 MuXq+ Iyy− Mq (Xu− m) − MwXqp1+ Mwp3(Xu− m) −B zcfcos(θ ) Zqp2(Xu− m) − p3(Xu− m) (Zw− m) + Xqp1(Zw− m)

(47)

5.2 Minimum time 29

and the switching functions are:

ψi= pTbi (5.11) that is,                                                                ψ1=1k(((m − Zw˙)(Iyy− Mq˙) − Zq˙Mw˙+ zp1(Xq˙(m − Zw˙)))p1 +(Zq˙Mu˙+ zp1(Zq˙(m − Xu˙)))p2 +(Mu˙(m − Zw˙) + zp1(m − Xu˙)(m − Zw˙))p3) ψ2=1k(((m − Zw˙)(Iyy− Mq˙) − Zq˙Mw˙+ zp2(Xq˙(m − Zw˙)))p1 +(Zq˙Mu˙+ zp2(Zq˙(m − Xu˙)))p2 +(Mu˙(m − Zw˙) + zp2(m − Xu˙)(m − Zw˙))p3) ψ3=1_k((Xq˙Mw˙− xp3(Xq˙(m − Zw˙)))p1 +((m − Xu˙)(Iyy− Mq˙) − Xq˙Mu˙− xp3(Zq˙(m − Xu˙)))p2 +(Mw˙(m − Xu˙) − xp3(m − Xu˙)(m − Zw˙))p3) ψ4=1_k((Xq˙Mw˙− xp4(Xq˙(m − Zw˙)))p1 +((m − Xu˙)(Iyy− Mq˙) − Xq˙Mu˙− xp4(Zq˙(m − Xu˙)))p2 +(Mw˙(m − Xu˙) − xp4(m − Xu˙)(m − Zw˙))p3) (5.12)

In figure5.4we verify that the computed Hamiltonian is always 1.

Figure 5.4: Hamiltonian

The transversality condition as seen in table5.13and plotted in5.5, is satisfied. Numerical results for the final state:

p5(tf) = −0.251899, p6(tf) = −0.0608948

x(tf) = 39.7827, z(tf) = 9.94746.

(48)

Figure 5.5: Final target and its multipliers

Plotting the switching functions against the controls we verify that they agree with the con-clusions of the maximum principle.

Figure 5.6: Switching function for each force

Thus the solution is partially validated. Our solution may be nevertheless a sub optimal solution then we should help the solver reach the optimal solution through different seeds for the final time or even adding restrictions. We investigate the optimality of the solution and, at the same time, test different control policies that would possibly be easier to implement in real vehicles. We perform various tests described next.

(49)

5.3 Minimum time for piece wise constant controllers 31

5.3 Minimum time for piece wise constant controllers

From the minimum time problem we get singular solutions. Full bang-bang solutions would allow us to test second order sufficient conditions. Also, we would like to make sure our thrusters take constant values.

The idea we provide in here is to look at the solution of the problem and find a constant control to replace the singular control.

Having all the thrusters bang-bang, except the thruster 3 in t ∈ [t1,t2] that is singular, we look

for a desired constant control ¯u3for [t1,t2]. This condition is implemented in the AMPL code as:

i f nt1 tf

≤ i ≤ nt2 tf

then u3(i) = ¯u3 (5.14)

Of course, the new tf will be larger than the previous computed one as we are adding

con-straints to the previous problem. However, this merely implies that somewhere after t2the solution

will re-adjust. We then want it to keep the piece wise constant properties.

For this reformulation, the optimal control problem is the same as in the minimum time problem. The only difference is that, from figure5.2, we establish that the singular parts have to be substituted by some constant controllers, as follows:

   i f 0.1 ≤ t ≤ 11 then u3(t) = ¯u3 i f 11.3 ≤ t ≤ 12.7 then u4(t) = ¯u4 (5.15)

We know that this problem differs from common literature in its formulation. In papers like [14], the problem is to find the minimum time solution with piece wise constant controllers as well, but, the problem is formulated as a (ST PP)p, meaning, a switching time parameterization

problem where p refers to the number of switching times.

However, we believe it is also an interesting result and we hope to develop a result as in the literature in future work.

5.3.1 Results

Here follow the numerical results:

n = 1000, tf = 14.3441, OBJ = 14.3441,

p₁(0) = 0.0623421, p₂(0) = 3.96739, p₃(0) = 1.83727, p4(0) = 1.32105, p5(0) = −0.288215, p6(0) = −0.0308863.

(5.16)

In figure5.7we get a clear view of the linear and angular velocities as well as the angle. The velocity in x appears to be just as expected beginning with a gradual increase in velocity, then, for the majority of time, constant velocity, finishing with a sharp braking. However, the velocity in

(50)

z, the angular velocity and the angle, appear to be again unstable varying between negative and positive values as if oscillating.

In figure5.8 we can see the plot of all the four controllers. The rear thrusters 1 and 2 are bang-bang and full-power accelerating for about 11 seconds, then 2 seconds of braking full-power. The upper thruster 3 is now bang-singular-bang. The upper thruster 4 is now bang-bang for most of the time with a constant singular arc before the end.

In figure5.9we can confirm what the previous figures suggested: the solution path is, once more, oscillating around a straight line. As expected, the path decomposed in x is approximately a straight line and the path decomposed in z is somehow oscillating.

(51)

5.4 Modified problem: Follow the line 33

5.4 Modified problem: Follow the line

Given that the solutions computed before are not straight lines, we want to check whether the computation program would find a better solution somewhere near the straight line. Thus, the initial and final condition for θ is changed to θ = −0.24 so that it begins and finishes perfectly aligned with a straight line trajectory, as in equation5.17.

Then, we add the constraint5.18to make sure the deviation from the given line path would not exceed a given tolerance, in this case, 10−3.

tan(θ ) = z x=

10

40 ⇔ θ = −0.24 (5.17)

|z(i) − 0.25x(i)| ≤ 0.001 (5.18) Note that this is an optimal control problem with state constraints and that the necessary conditions of optimality are now different from those of the initial problem. Only through these state constraints can we force the system to follow a linear trajectory.

Regarding the generic problem (P) of the previous chapter, our h(x(t)) will be, for this prob-lem:

h(x) = |z(i) − 0.25x(i)| − 0.001 (5.19)

(52)

detailed model in chapter4. Also, f2refers to the vector f2in chapter4as well.

(O.P)                                                minimise tf s.t. ˙x1(t) = f1det(x1(t)) + 4

∑

i=1 bidet× ui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t)) (u1(t), u2(t), u3(t), u4(t)) ∈ [−30, 30] × [−30, 30] × [−30, 30] × [−30, 30] (u(0), w(0), q(0), θ (0), x(0), z(0)) = (0, 0, 0, −0.24, 0, 0) |z(t) − 0.25x(t)| − 0.001 ≤ 0 a.e. t ∈ [0,tf], (u(tf), w(tf), q(tf), θ (tf)) = (0, 0, 0, −0.24) (x(tf) − 40)2+ (z(tf) − 10)2≤ 0.05 (5.20) 5.4.1 Results

n = 1000, tf = 13.0914, OBJ = 13.0914,

p1(0) = −0.536971, p2(0) = −0.0630637, p3(0) = −0.0213961,

p₄(0) = −0.0928452, p₅(0) = −0.128303, p₆(0) = −0.56124.

(5.21)

In figure5.10we can see that the linear and angular velocities, as well as the angle are more stable than in the previous cases. However, even though the angle is set to match the whole path, we can see how it still varies from about -0.1 to -0.4.

(53)

5.4 Modified problem: Follow the line 35

As in figure5.11, thruster 3 and 4 have clearly become more stable during the high speed fraction of the trajectory. However, for the acceleration and braking parts, the forces are still unstable.

Finally, the physical path resulting as in figure5.12is considerably close to a straight line, as desired. Just as the trajectory decomposed in z, in comparison to the result of the previous solution.

5.4.2 Validation

This problem reformulation includes state constraints, and, because of that, the validation through maximum principle is out of the scope of this thesis. However, if the state constraint does not

(54)

coincide with its limits, meaning, if the trajectory does not touch the boundary of the state con-straint, then we could use maximum principle stated before. So, we zoomed in on the trajectory (in blue) and added the state path constraints (in red) to verify whether the path coincides with the constraints. Such result follows:

Figure 5.13: Zoom on the trajectory in x,z

We can see how it’s not clear whether the trajectory touches the boundary.

Nevertheless, we dare to present the graph of the switching functions. Note that more exten-sive analysis will be the subject of future research.

Figure 5.14: Switching functions for each force

As seen in 5.14, the switching functions appear to be negative when the control is at its minimum, positive when the control is at its maximum and zero, with a given tolerance, when the control is singular.

(55)

5.5 Modified problem: Thrusters limitation 37

Our main goal was to see whether a trajectory with a straight line would reveal a better solution. This was not the case. However, the difference between the result for this objective function and the previous one is about 0.15. Thus, this seems to be a sub optimal solution and it helps to convince us of the optimality of the solution first computed.

5.5 Modified problem: Thrusters limitation

Now, we look for solutions with bang-bang controls. We do that by constraining u3and u4to the

sets [0,30].

(T.L.P)                                          minimise tf s.t. ˙x1(t) = f1det(x1(t)) + 4

∑

i=1 bidetui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t)) (u1(t), u2(t), u3(t), u4(t)) ∈ [−30, 30] × [−30, 30] × [0, 30] × [0, 30] (u(0), w(0), q(0), θ (0), x(0), z(0)) = (0, 0, 0, 0, 0, 0) (u(tf), w(tf), q(tf), θ (tf)) = (0, 0, 0, 0) (x(tf) − 40)2+ (z(tf) − 10)2≤ 0.05 (5.22) 5.5.1 Results

n = 1000, tf = 14.2122, OBJ = 14.2122,

p₁(0) = −0.524852, p₂(0) = −0.0774616, p₃(0) = 0.0231503, p4(0) = −0.117186, p5(0) = −0.213612, p6(0) = −0.180204.

(5.23)

The computed solution is a sub optimal solution. However, it may be an important one since it is bang-bang. In figure5.15the linear and angular velocities appear to be as intuitively expected. Meaning, they are constant along the path with acceleration in the beginning and braking at the end.

Analysing the forces of the thrusters in figure5.16we can see that, in fact, the controls are bang-bang for the whole trajectory. Thruster 1 and 2 are in full positive power for the whole trajectory and then brake to make the vehicle stop, following by half a second of acceleration to compensate. Thruster 3 does not seem to be of great use. Thruster 4 has but one job: to lower the front of the vehicle at the very beginning so that the angle matches the desired trajectory.

(56)

In figure5.17we can confirm what the previous figures suggested: the solution path is not straight nor wobbling, but a steady elongated curve as a "tamer" version of the brachistochrone.

This solution is far from the optimal. However, it is the only solution, so far, with only bang-bang controllers full time. Thus, so far, if one would want only bang-bang-bang-bang controllers, this could be the best so far. However, we need validate this possibly optimal solution with the maximum principle stated in4. This we present next.

5.5.2 Validation

For this formulation we have the same necessary conditions as well as switching functions as in the minimum time problem since the only difference is in the constraints of the controls of thrusters 3 and 4.

(57)

5.5 Modified problem: Thrusters limitation 39

In figure5.18we verify that the computed Hamiltonian is always 1.

Figure 5.18: Hamiltonian

Second of all, we validate the transversality condition −p(tf) ∈ NT(¯x(tf)) as seen in table 5.24and plotted in5.19.

Numerical results for the final state:

p5(tf) = −0.213612, p6(tf) = −0.180204

x(tf) = 39.8291, z(tf) = 9.85582.

(5.24)

Finally, we validate the switching functions as the condition (iv) implies. As seen in5.20, the switching functions are negative when the control is at its minimum, positive when the control is at its maximum and zero, when the control is singular.

(58)

Figure 5.19: Final target and its multipliers

Figure 5.20: Switching function for each force

Thus the solution is partially validated. Even though its a sub optimal solution, it exhibits a small number of switching times.

(59)

5.6 Minimum Energy and time 41

5.6 Minimum Energy and time

Now we want to smooth the controls for AUV implementation. To do so, we consider now the minimum energy and time problem:

(E.O.P)                                              minimise Z t_f 0 1 + 0.001 4

∑

i=1 u2_i(t) ! dt. s.t. ˙x1(t) = f1det(x1(t)) + 4

∑

i=1 bi× ui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t)) (u1(t), u2(t), u3(t), u4(t)) ∈ [−30, 30] × [−30, 30] × [−30, 30] × [−30, 30] (u(0), w(0), q(0), θ (0), x(0), z(0)) = (0, 0, 0, 0, 0, 0) (u(tf), w(tf), q(tf), θ (tf)) = (0, 0, 0, 0) (x(tf) − 40)2+ (z(tf) − 10)2≤ 0.05 (5.25) Note how our cost function is now on the Bolza form:

minimise Z t_f 0 ( 1 + 0, 001 4

∑

i=1 u2_i(t) ) dt. (5.26)

We need to rewrite it in the Mayer form. To do so, we apply the so called state augmentation technique: we add an extra state y,

˙ y= 1 + 4

∑

i=1 αiu2i y(0) = 0

for which we define αi= 0.001 for i = 1, 2, 3, 4 as to get a final time as closest as possible to

the original problem while maintaining the energy optimisation.

(60)

42 Numerical Results (E.O.P)                                                          minimise y s.t. ˙x1(t) = f1det(x1(t)) + 4

∑

i=1 bi× ui(t) a.e. t ∈ [0,tf], ˙x2(t) = f2(x1(t)) ˙ y= 1 + 4

∑

i=1 αiu2i (u1(t), u2(t), u3(t), u4(t)) ∈ [−30, 30] × [−30, 30] × [−30, 30] × [−30, 30] (u(0), w(0), q(0), θ (0), x(0), z(0)) = (0, 0, 0, 0, 0, 0) y(0) = 0 (u(tf), w(tf), q(tf), θ (tf)) = (0, 0, 0, 0) (x(tf) − 40)2+ (z(tf) − 10)2≤ 0.05 (5.27) 5.6.1 Results

For this modified problem the objective functional final time is 13.1745. This objective function is a bit higher than in the original problem (about 0.2 seconds higher). However, that is a very small distance for the real-life solution compared to the optimal or sub optimal solution.

n = 1000, tf = 13.1745, OBJ = 15.4171,

p1(0) = −0.641147, p2(0) = −0.180245, p3(0) = 0.0346419,

p4(0) = −0.0440211, p5(0) = −0.297976, p6(0) = −0.0773669.

(5.28)

(61)

5.6 Minimum Energy and time 43

In figure5.21we can see how all the velocities are "tamed" in comparison to previous results with no significant bouncing or sinusoidal-like effects.

Analysing the forces of the thrusters in figure 5.22we see the influence of the derivative limitation as the steepest descents never surpass 14.71 N/sec. Thrusters 1 and 2 are similar to previous results since they first accelerate for a long period of time and then incrementally brake. Thrusters 3 and 4 are now smooth functions.

In figure5.23we can analyse the resulting trajectory. At the expense of half a second we obtain a smoother trajectory as well smoother controls, if varying controls are possible to imple-ment.

Optimal control of AUVs

F

E

U

P

Optimal control applied to AUVs

Joana Filipa Gouveia Fonseca

Resumo

Abstract

Acknowledgements

Contents

List of Figures

List of Tables

Abbreviations

Chapter 1

Introduction

1.1

Problem Statement

1.2

Goals and motivation

1.3

Report’s structure

Chapter 2

State of the art

2.1

AUV: Mares

2.2

Optimal Control and Trajectory Planning

2.3

Discretization methods

2.4

Computational tools for optimisation

2.5

Optimality validation: Pontryagin’s Maximum Principle

Chapter 3

Optimal control

∑

∑

3.1

Minimum Time Problem

∑

∑

∑

∑

∑

∑

∑

∑

∑

3.2

Minimum Time Energy Problem

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

∑

Chapter 4

Modelling

4.1

Detailed model

∑

4.2

Simplified model

∑

Chapter 5

Numerical Results

5.1

Introduction