c© 2017 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in anycurrent or future media, including reprinting/republishing this material for advertising or promotional purposes, creating newcollective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in otherworks.

Conference: IEEE OCEANS 2014 - TAIPEI , 7-10 April 2014

DOI: 10.1109/OCEANS-TAIPEI.2014.6964475

URL: http://ieeexplore.ieee.org/document/6964475/

arX

iv:1

707.

0276

7v1

[cs

.RO

] 1

0 Ju

l 201

7

Model Identification and Controller ParameterOptimization for an Autopilot Design for

Autonomous Underwater VehiclesRalf Taubert, Mike Eichhorn, Christoph Ament

Inst. for Automation and Systems Engineering IlmenauUniversity of Technology98693 Ilmenau, Germany

Email: {ralf.taubert, mike.eichhorn, christoph.ament}@tu-ilmenau.de

Marco Jacobi, Divas Karimanzira, Torsten PfuetzenreuterAdvanced System Technology (AST)

Branch of Fraunhofer IOSB98693 Ilmenau, Germany

Email: {marco.jacobi, divas.karimanzira, torsten.pfuetzenreuter}@iosb-ast.fraunhofer.de

Abstract—Nowadays an accurate modeling of the system to becontrolled is essential for reliable autopilot.This paper presents a non-linear model of the autonomousunderwater vehicle “CWolf”. Matrices and the correspondingcoefficients generate a parameterized representation for addedmass, Coriolis and centripetal forces, damping, gravity and buoy-ancy, using the equations of motion, for all six degrees of freedom.The determination of actuator behaviour by surge tests allows theconversion of propeller revolutions to the respective forces andmoments. Based on geometric approximations, the coefficients ofthe model can be specified by optimization algorithms in “openloop” sea trials.The realistic model is the basis for the subsequent design of theautopilot. The reference variables used in the four decoupledadaptive PID controllers for surge, heading, pitch and heave areprovided a “Line of Sight” - guidance system. A constraint crite-ria optimization determines the required controller parameters.The verification by “closed loop” sea trials ensures the results.

Index Terms—AUV; Water Quality; Modeling; Identification;Controller Design; Autopilot; Line of Sight; Decoupled AdaptivePID; Optimization

I. INTRODUCTION

Fish farming in aquacultures is a sensible alternative toindustrial fishing, which is accompanied by the loss of biodi-versity, the exploitation of global fisheries and the increasingpollution of the oceans. In 2010 more than 60 million tonsof fish, mussels and crabs came from such farms and about600 species of animals are kept in aquacultures worldwide.The food sector, which has been growing fastest in recentyears, has also its drawbacks. Feeding the animals and theirexcreta causes over-fertilization of the surrounding waters.Thus, the nutrient content increases in rivers, lakes and bays,destabilizing the natural ecosystem and endangering the localfauna and flora [1].

This work was supported by the European Regional Development Fund(ERDF) of the European Union via the Thuringian Coordination Office TNA#TNA VIII-1/2011

A. Project “SALMON”

Within the framework of the research project “ SALMON”(Sea Water Quality Monitoring and Management), funded bythe European Regional Development Fund (ERDF) [2], anautonomous underwater vehicle (AUV) records and analyseswater quality data. During planned missions in the vicinityof a fish farm, water samples are collected and analysed. Inaddition to the nitrate pollution, density, conductivity and saltconcentration are determined [3].The autopilot, the simulators and the mission planning soft-ware are developed by the Department of Systems Analysisat the Ilmenau University of Technology [4]. The construc-tion and management of the AUV hardware components isconducted by the engineers at the Fraunhofer IOSB-AST[5]. A sensor module records the measurement data, whichis developed and maintained by 4H Jena GmbH [6]. TheNorwegian Institute of Marine Research [7] provides the testenvironment around an experimental fish farm off the islandof Austevoll in Hordaland (Norway) to carry out the final seatrials.

B. AUV Base Platform “CWolf”

The autonomous underwater vehicle (AUV) “CWolf”,whose performance parameters are shown in Table I, isbased on the remote operated vehicle (ROV) “SeaWolf” fromATLAS Hydrographic GmbH [8]. After modifying it to an

TABLE IPERFORMANCE PARAMETER OF AUV “CWOLF”

Parameter Value

Length 2.50 mDiameter 0.30 mWeight in air 135.00 kgMax. speed 6.00 knEndurance at 3.00 kn 3.00 hPayload 15.00 kg

autonomous platform, the control system ConSys is integrated[9]. By means of the modular payload concept, different sensorsystems can be installed inside and outside. Communicationtakes place with WLAN or fibre optic cable. Fibre optic gy-roscope (FOG), Doppler Velocity Log (DVL), scanning sonarand Global Position System (GPS) detect the current positionand orientation of the vehicle for navigation. The AUV isactuated by four stern propellers and two vertical thrusters.Different activations of individual propellers by steering andpitching commands allow the manoeuvring in horizontal andvertical plane. Thus, it is possible to pilot the vehicle withoutrudder units during forward movement and hovering. Thevertical thrusters are used exclusively for vertical movement.Figure 1 shows the construction model of the AUV ”CWolf”with components:

1) stern propellers2) sensor module3) Global Position System (GPS)4) bow section5) stern section6) vertical thruster stern7) payload section8) Doppler Velocity Log (DVL)9) vehicle guidance system (VGS)

10) vertical thruster bow11) scanning sonar

II. MODELING

The non-linear continuous equation of motion (6) withsix degrees of freedom (6DOF) serves as a mathematicaldescription of the movement of an underwater vehicle in afluid. The force-moment vector τ summarizes the various

physical influences as a function of acceleration ν, velocityν and position η [10].

τ = [X Y Z K M N ]T (1)

ν = [u v w p q r]T (2)

η = [x y z φ θ ψ]T (3)

The relative velocity of the vehicle νr is used for calculationof the hydrodynamic terms. It’s calculated by the subtractingthe current velocity of the liquid νc from the absolute velocityν [11].

νr = ν − νc (4)= [u− uc v − vc w − wc p q r]

T (5)

Figure 2 presents the schematic representation of the systemmodel according to (6), which consists of several modules,explained in the following sections.

Fig. 2. Block diagram of AUV model

Fig. 1. AUV “CWolf” outer layout

ν = [MRB +MA]−1 · [B(ν) uA − CRB(ν)ν − CA(νr)νr −D(νr)νr − g(η)] · Vconst (6)

A. KinematicsDepending on the available sensors, the navigation system

provides position, orientation and motion data, which aredetermined in different reference systems. The transformationmatrix is necessary for the conversion of the body-fixed (Bf)ν frame values into earth-fixed (Ef) η frame values [12].

η = J(η)ν (7)

B. DynamicsThe hydrodynamic behaviour of the vehicle is described by

matrices, which in combination with the movement vectorsrepresent forces and moments. Simplifications, such as ne-glecting the coupling terms and the reduction to diagonal ma-trices, depend on various assumptions (eg. rotation-symmetricbody shapes, low speed, high depth and insignificant current,etc.) [13].

Rigid-body inertia matrix MRB

The distribution of mass m in a rigid body and its relativedistance to the center of gravity cause inertia and moments ofinertia, which are combined in the constant matrix MRB .

MRB =

[m I3×3 −m S(rg)m S(rg) IO

](8)

The position vector rg indicates the distance between the body-fixed origin (O) and the center of gravity (CG) of the vehicle.The calculation of the cross product of the mass and theposition vector is computed by the skew-symmetric matrixS(.). The inertia tensor IO describes the moments of inertiawith respect to all axes of the body.

IO =

Ix −Ixy −Ixz−Iyx Iy −Iyz−Izx −Izy Iz

(9)

Assuming that the center of gravity lies in the body-fixedorigin (CG = O) and the deviation moments for rotation-symmetrical bodies are zero (Ixy = Ixz = Iyz = 0), theinertia matrix is reduced to a diagonal matrix .

MRB = −diag {m m m Ix Iy Iz} (10)

The geometrical calculation of a homogeneous straight cylin-der approximates the outer shape of the vehicle. By extendingthe formula with the coefficients CIx , CIy , the subsequentoptimization is adjusting the model.

Ix =

∫V

(y2 + z2) · ρM dV

= CIx · 1/2 ·m · r2 (11)

Iy =

∫V

(x2 + z2) · ρM dV

= CIy · 1/12 ·m · (l2 + 3 · r2) (12)Iz = Iy (13)

The geometric parameters are determined by the radius r, thelength l and the volume V of the vehicle. For the materialdensity ρM , a homogeneous distribution is assumed.

Added mass matrix MA

By accelerating the vehicle, the surrounding fluid producesadditional forces and moments. For underwater vehicles, asimplification to diagonal matrix is done analogous to theinertia matrix.

MA = −diag {Xu, Yv, Zw,Kp,Mq, Nr} (14)

By using a prolate ellipsoid to approach the form of theouter vehicle hull [10], a direct correlation between the addedmass coefficients and vehicle mass (inertia) can be found. Thefactors CXu , CYv CZw , CMq

and CNr are optimized.

Xu = −CXu(2/3πρF r2l) = −CXum (15)

Yv = −CYv (2/3πρF r2l) = −CYvm (16)

Zw = −CZw(2/3πρF r2l) = −CZwm (17)

Mq = −CMq (2/15πρF r

2l)(1/4l2 + r2) = −CMqIy (18)

Nr = −CNr (2/15πρF r

2l)(1/4l2 + r2) = −CNrIz (19)

The added mass for roll cannot be used for the approach ofprolate ellipsoid (Kp 6= 0). Due to additional structures at theouter hull of the vehicle (fins and tower), a factor CKp

mustbe introduced.

Kp = −CKpm (20)

Rigid-body Coriolis and centripetal matrix CRB(ν)

The rigid body Coriolis and centripetal terms are derivedfrom the rigid body inertia matrix MRB . They are connectedto the body-fixed velocity of the vehicle ν as in [12].

CRB(ν) =[

03×3 −mS(ν1)−mS(ν2)S(rg)

−mS(ν1)+mS(rg)S(ν2) −S(IOν2)

](21)

For a concentrated representation the body-fixed velocity vec-tor ν is divided in ν1 = [u v w]

T and ν2 = [p q r]T .

Hydrodynamic Coriolis and centripetal matrix CA(νr)

The hydrodynamic Coriolis and centripetal terms also resultfrom the added mass matrix MA and are linked with therelative body fixed velocity νr via cross product.

CA(νr) =

0 0 0 0 −Zwwr Yvvr

0 0 0 Zwwr 0 −Xuur

0 0 0 −Yvvr Xuur 0

0 −Zwwr Yvvr 0 −Nrr Mqq

Zwwr 0 −Xuur Nrr 0 −Kpp

−Yvvr Xuur 0 −Mqq Kpp 0

(22)

Damping matrix D(νr)

The hydrodynamic damping, that acts on the vehicle, can bereduced to linear and quadratic terms. The influence of windand waves is neglected. Only the energy loss due to friction(laminar and turbulent) and the viscosity of the water shouldbe considered at this point.

D(νr) = −diag {Xu, Yv, Zw,Kp,Mq, Nr} (23)−diag

{Xu|u|, Yv|v|, Zw|w|,Kp|p|,Mq|q|, Nr|r|

}|νr|

The linear terms are neglected in most publications (eg. [14]),because they are minuscule in the forward velocity with rangeof 0, 5 ≤ u ≤ 2, 5 m/s. The following optimization of theseparameters shows that this assumption is not useful in all cases.In general the square damping term depends on the materialdensity ρF , the drag coefficient CD and the projection area.The quadratic damping coefficient by frontal flow Xu|u| isdetermined by the surface in yz-plane Ayz .

Xu|u| = −1/2 · ρF · CDx ·Ayz (24)

Assuming an ellipsoidal hull shape (projection area: Ayz =π·r2) and applying the laminar flow theory, the drag coefficientCDx can be identified by the friction coefficient Cf [15].

CDx = 0, 442r

l+ 4Cf

l

2r+ 4Cf

(2r

l

)1/2

(25)

With knowledge of the Reynolds number Re, the frictioncoefficient Cf is extracted from the ITTC-curve [16].

Cf =0, 075

(log10Re− 2)2(26)

The damping in the x- and y-direction is approximated by thelateral flow of a short circular cylinder (Axy = Axz = 2 · r · l)[17].

Yv|v| = −1/2 · ρF · CDy

∫ l/2

−l/2

2r dx

= −1/2 · ρF · CDy ·Axz (27)

Zw|w| = −1/2 · ρF · CDz

∫ l/2

−l/2

2r dx

= −1/2 · ρF · CDz ·Axy (28)

To calculate the damping moments a homogeneous straightcircular cylinder is used as basis. For this application, anadditional drag coefficient for the rotation must be introduced[18].

Mq|q| = −1/2 · ρF · CDz · CDq ·∫ l/2

−l/2

|x|3 · 2r dx

= −1/32 · ρF · CDz · CDq · r · l 4 (29)

Nr|r| = −1/2 · ρF · CDy · CDr ·∫ l/2

−l/2

|x|3 · 2r dx

= −1/32 · ρF · CDy · CDr · r · l 4 (30)

Analogous to the calculations of the added mass the rotationabout the x-axis cannot be neglected (Kp|p| 6= 0). Therefore,

the projection areas of additional structures on the outer hullare summarized in AR = 4 · (2rB · rB)︸ ︷︷ ︸

fin

+ 2 · rB · rB︸ ︷︷ ︸tower

.

Kp|p| = −1/2 · ρF · CDp ·AR (31)

Gravity and buoyancy matrix g(η)

The weight W and the buoyancy B of all vehicle sectionsare critical for calculating the restoring forces and momentsg(η). The transformation J−11 (η2) converts the body-fixedvalues to the required earth-fixed relationships.

g(η) = −

J−11

(η2)

(0

0

W

)+J

−11

(η2)

(0

0

B

)rg×J

−11

(η2)

(0

0

W

)+rb×J

−11

(η2)

(0

0

B

) (32)

Since the respective forces apply at the center of gravity rg =

[xg yg zg]T and the center of buoyancy rb = [xb yb zb]

T ,the exact location of the centers must be known.

g(η) = −

(W−B) sin θ

−(W−B) cos θ sinφ

−(W−B) cos θ cosφ

−(ygW−ybB) cos θ cosφ+(zgW−zbB) cos θ sinφ

(zgW−zbB) sin θ+(xgW−xbB) cos θ cosφ

−(xgW−xbB) cos θ sinφ+(ygW−ybB) sin θ

(33)

C. Actuators

The central element of modeling with the 6DOF - equationof motion is the representation of all effects in the force-moment vector τ . A control vector uA, containing for exampledesired revolution of the propellers ndP , serves as an interfaceto the actuators (propellers, thrusters, rudders, etc.) of thevehicle.The Matrix B (Fig. 3) transforms the desired values into thegenerated forces and moments. For the transformation thestatic/dynamic behaviour of the motor controller, the character-istic curve of the propeller in water and the position/orientationof the actuator need to be considered.

Fig. 3. Block diagram of actuator matrix

Static behaviour

The motor controller limits the desired revolution speednd to a negative n−max and positive maximum n+max. In therevolution speed range between a negative n−min and positivelow limit n+min no stable values for actual revolution speed nacan be guaranteed.

na =

n+max, for nd ≥ n+

max

n+min

, for nd > 0

and nd < n+min

0, for nd = 0

n−min

, for nd < 0

and nd > n−min

n−max, for nd ≤ n−

max

nd, otherwise

Fig. 4. Static transfer behaviour of the motor controller

Dynamic behaviour

The increase nacc and decrease ndec of the desired revolu-tion speed is limited to reduce the load by high motor currentsdue to rapid and large revolution speed changes.

In addition, the dynamic transfer behaviour of the closed

Fig. 5. Dynamic limiting of increasing and decreasing

control loop of the motor controller system leads to a delay inthe time domain. The command action of the PID controllercan be approximated using a PT1 element with time delay.

G(s) =KV

(T1s+ 1)e−Tts (35)

0 0.5 1 1.5 2 2.5 3 3.5 4 4.50

200

400

600

800

1000

1200step response motor controller (dynamic behaviour)

revo

lutio

n [r

pm]

time [s]

parameter

KV = 1

Tt = 8.7387e-05s

T1 = 0.0959s

nm = 278.3538rpm

desired revolutionlimited desired revolutionmesured revolutionmodeled revolution

Fig. 6. Modelled and measured step response of revolution

Motor characteristic

The relationship between the actual revolution speed nP andthe propulsive force generated FP is declared by the charac-teristic curve, which reflects the behaviour of the propellerin the water. According to [10] this non-linear curve can berepresented as a polynomial of second order. The coefficientsα1, α2 and the diameter of the propeller dP describe thehydrodynamic and structural properties. The density ρF andthe velocity of the fluid Va are also included in the equation.

FP = α1 · ρF · dP 4︸ ︷︷ ︸p1

·|nP |nP +α2 · ρF · dP 3 · Va︸ ︷︷ ︸p2

·|nP | (36)

This quadratic relationship is determined by surge tests. Forthis purpose, the AUV was fixed in a suitable depth of atest basin with sufficient distance from the edge of the pool,to prevent the vertical and horizontal pulling of the vehicleduring acceleration (Fig. 7). A load cell measures the over

Fig. 7. Surge test

deflection pulleys and haulage ropes acting force generatedby the respective actuators. In four experiments, the entirerevolution range of all actuators in both directions can bescanned:• positive x-direction (stern propeller forward)• negative x-direction (stern propeller backward)• positive z-direction (vertical thruster downward)• negative z-direction (vertical thruster upward)

Afterwards the total forces obtained must be separated into theindividual components of each actuator (see section “positionand orientation”). The desired quadratic relationship betweenrevolution (measured value matrix UR) and force of a sin-gle actuator (output signals yR) is determined by the directregression using a second order model.

yR = a0 +

k∑i=1

aiui +

k−1∑i=1

k∑j=i+1

aijuiuj +

k∑i=1

aiiu2i (37)

aR = [UTR UR]−1 UT

R yR (38)

−2000 −1500 −1000 −500 0 500 1000 1500 2000−100

−50

0

50

100

150propeller characteristic (revolution −−> force)

forc

e [N

]

revolution [rpm]

forward

a0 = 0

a1 = 0

a2 = 3.2845e−05

B = 0.99723

backward

a0 = 0

a1 = −0.0059059

a2 = −2.2203e−05

B = 0.99977

measureregression

Fig. 8. Motor characteristic of stern propeller estimated by direct regression

Due to the negative and positive revolution speed behaviour,a parameter vector aR is estimated for the propellers andthe vertical thrusters in both directions. The coefficient ofdetermination B evaluates the quality of the regression fromthe correlation of the measured and calculated values.

B = Kor(y, y)2 =

(Cov(y, y)

σy σy

)2

=

∑(y − y)2∑(y − y)2

(39)

Based on the determined motor characteristics for the sternpropellers (Fig. 8) and the vertical thrusters (Fig. 9), it ispossible to assign a force generated by the actuator to anyactual revolution speed in the range −2000...2000 rpm

Position and orientation

The force-moment vector τ depends on the arrangement ofthe actuators with respect to the vehicle center of gravity.The individual forces of the stern propellers (FSbU , FSbL, FPU

and FPL) and of the vertical thrusters (FV TB and FV TS),determined by the surge test, act in the directions shown inFig. 10. The inclination angle of the propellers αP and βPcomplicate the trigonometric relationships.

fSbU = FSbU ·[

+ sin (90◦−βP )·sin (90◦−αP )

+ sin (90◦−βP )·cos (90◦−αP )

− cos (90◦−βP )

](40)

fSbL = FSbL ·[

+ sin (90◦−βP )·sin (90◦−αP )

+ sin (90◦−βP )·cos (90◦−αP )

+ cos (90◦−βP )

](41)

fPU = FPU ·[

+ sin (90◦−βP )·sin (90◦−αP )

− sin (90◦−βP )·cos (90◦−αP )

− cos (90◦−βP )

](42)

fPL = FPL ·[

+ sin (90◦−βP )·sin (90◦−αP )

− sin (90◦−βP )·cos (90◦−αP )

+ cos (90◦−βP )

](43)

fV TB = FV TB ·[

0

0

1

](44)

fV TS = FV TS ·[

0

0

1

](45)

With the distance vectors of the stern propellers (rSbU , rSbL,

−2000 −1500 −1000 −500 0 500 1000 1500 2000−4

−2

0

2

4

6

8

10vertical thruster characteristic (revolution −−> force)

forc

e [N

]

revolution [rpm]

downward

a0 = 0.12579

a1 = 0

a2 = 2.1909e−06

B = 0.99891

upward

a0 = −0.3406

a1 = 0.00075141

a2 = −3.3546e−07

B = 0.95929

measureregression

Fig. 9. Motor characteristic of vertical thruster estimated by direct regression

rPU and rPL) and of the vertical thrusters (rV TB and rV TS),it is possible to calculate the resultant moments. The followingaddition of the force-moment vectors τ1 and τ2 forms a vectorthat contains all influences of the actuators.

τ =

[f

r × f

]= τ1 + τ2 (46)

τ1 =[

fSbU+fSbL+fPU+fPL

rSbU×fSbU+rSbL×fSbL+rPU×fPU+rPL×fPL

](47)

τ2 =[

fV TB+fV TS

rV TB×fV TB+rV TS×fV TS

](48)

Fig. 10. Force direction of stern propellers and vertical thrusters

D. Optimization

The optimization of the model parameters is carried out bythe improvement of associated coefficients C... according tothe schema shown in Fig. 11.The start parameters are based on geometric approximations

Fig. 11. Schematic design of parameter optimization

and are placed within the certain limits of optimization. Thedata recorded in sea trials serve as a reference and provide thecontrol variable vector for the simulation of the model. On themodel output, the model error e is calculated by comparing theof measured y(t) and simulated data y(t). The least absoluteerror criterion determines the quality of the current parameterset Q.

Q =

∫|e(t)| dt =

∫|y(t)− y(t)| dt (49)

The optimization algorithm seeks the global minimum of thequality by the means of a evolution strategy and uses a simplexalgorithm to increase the convergence speed while doing thefine search. The parallel optimization is divided into fourstages. Individual force or torque components are consideredindependently with the vector Vconst (see Fig. 2) to preventthe mutual coupling effects.

0 10 20 30 40 50 60 70 80 90 100

0

0.5

1

1.5

2

optimization Fx − surge

velo

city

OG

Bf u

[m/s

]

time [s]

optimal parameter

Ix = 1, 6451 kg m2

Xu = −96, 2196 kg

Kp = −526, 3126 kg

Xu = −1, 7404e−08 kg m2

Kp = −13, 0334 kg m2

Xu|u| = −48, 7573 kg

m

Kp|p| = −57, 9906 kg

m

Fx = 100 N measured signalFx = 100 N modeled signalFx = 200 N measured signalFx = 200 N modeled signal

Fig. 12. Optimization of the forward speed with stern propellers

0 50 100 150 200 250

0

1

2

3

4

5

6

7

optimization Fz − heave

time [s]

dept

h z

[m]

optimal parameter

Zw = −74, 8014 kg

Zw = −35, 9391 kg m2

Zw |w | = −160, 2293 kg

m

Bpayload = 40, 2744 N

Fz = 15, 00...9, 35 N measured signalFz = 15, 00...9, 35 N modeled signal

Fig. 13. Optimization of the diving with vertical thrusters

0 5 10 15 20 25 30 35 40−160

−140

−120

−100

−80

−60

−40

−20

0

optimization Fx T

z − surge & yaw

time [s]

ψ [°

]

optimal parameter

Iy = 109, 0332 kg m2

Yv = −74, 8014 kg

Nr = −170, 6934 kg

Yv = −20, 4470 kg m2

Nr = −0, 0489 kg m2

Yv|v| = −559, 4828 kg

m

Nr|r| = −3741, 7594 kg

m

Fx = 50 N Tz = 10 N m measured signalFx = 50 N Tz = 10 N m modeled signalFx = 80 N Tz = −10 N m measured signalFx = 80 N Tz = −10 N m modeled signal

Fig. 14. Optimization of the turning with stern propellers

0 5 10 15 20 25−18

−16

−14

−12

−10

−8

−6

−4

−2

optimization Fx T

y − surge & pitch

time [s]

θ [°

]

optimal parameter

Mq = −3701, 2847 kg

Mq = −0, 0158 kg m2

Mq|q| = −29591, 9015 kg

m

zb = −0, 0404 m

Fx = 200 N Ty = −30 N m measured signalFx = 200 N Ty = −30 N m modeled signalFx = 100 N Ty = −20 N m measured signalFx = 100 N Ty = −20 N m modeled signal

Fig. 15. Optimization of the diving with stern propellers

III. CONTROLLER DESIGN

The presented autopilot consists of a waypoint guidance,based on the principle of “Line of Sight” (LoS) and fourdecoupled adaptive PID controllers (Fig 16).

Fig. 16. Block diagram of control loop

After calculating the desired values (ηd, νd) from thespecified waypoints wp, the control deviation (ηe, νe) isformed by the measured navigation data (η, ν). The adaptivePID controller generates the control variable vector uA, bymeans of the inverse actuator matrix B−1 which serves as theinterface to the vehicle.

uA = B−1 [τPID(νe) + g(η)] (50)= B−1

[JT (η2) τPID(ηe) + g(η)

](51)

The additive combination of the restoring forces and momentsg(η) is used as the control variable intrusion, because theseinfluences are known a priori.

A. Guidance control

The guidance system generates a “Line of Sight” (LoS)vector from the available waypoints wpk = [xk yk zk]

T ,continuously reducing the cross track error xte. The 3D-LoScalculates the heading angle ψd and the pitch angle θd, usingthe arc tangent function [19].

θd = atan2 (zk − z(t), xk − x(t)) (52)ψd = atan2 (yk − y(t), xk − x(t)) (53)

The switchover to the next waypoint occurs when the vehicleenters the sphere of acceptance with the radius R0.

[xk − x(t)]2

+ [(yk − y(t)]2

+ [(zk − z(t)]2 ≤ R 20 (54)

Fig. 17. Waypoint guidance with “Line of Sight”

B. Adaptive PID-Controller

The controlled systems offer no constant behaviour, espe-cially while an autopilot operates in closed loop. Due to thedependence of vehicle speed and acceleration it’s useful tovariably adapt the controller behaviour. A characteristic withsix supporting points changes the control parameters (KP , KI ,KD) in the velocity range of −5, 0 m/s ≤ ud ≤ 5, 0 m/s.

τPID = KP (ud) · eR(k)

+KI(ud) ·k−1∑i=0

eR(i)

−KD(ud) · [yR(k)− yR(k − 1)] (55)

The stable desired velocity ud is used instead of the fluctu-ating actual value u to avoid instabilities during parameteradaptation. In addition, only the negative controlled variabley is used for the calculation of the derivative term of the PIDcontroller in order to prevent an excessive response in suddenchanges.

Fig. 18. Structure of adaptive PID-controller with additional features

For stability and accuracy, controller functions like anti-windup, feed forward, reset bias and switch structure areintegrated in the decoupled adaptive controller [20].

C. Constraint Criteria Optimization

The command action of the control loop is provided bythe quality criteria. The constraints are specified with theNonlinear Control Design (NCD) - Blockset for Simulink. Bydefining the lower Cu(t) and upper constraint Co(t) betweena set point response y(t) has to run, the controller parameterscan be optimized, using the Fuzzy Control Design (FCD) -Toolbox for MATLAB.

In violation of a constraint by the signal trajectory, a flag(US(t), OS(t)) is activated.

US(t) ={

0, for Cu(t) < y(t)

1, for Cu(t) ≥ y(t)(56)

OS(t) ={

0, for Co(t) > y(t)

1, for Co(t) ≤ y(t)(57)

Fig. 19. Constraint criteria with error areas

The quality of the current controller parameter set Q iscalculated from the weight factor of constraint violation (WIu,WIo) [21].

Q =

∫ t

0

(US(t)·WIu·(Cu(t)−y(t)) + ...

OS(t)·WIo·(y(t)−Cu(t))

)dt (58)

Figure 20 shows the constraint criteria optimized velocity inthe x-direction u, changing by different set points ud.

0 50 100 150

0

0.5

1

1.5

2

2.5

Time (s)

v u (m

/s)

Fig. 20. Speed control optimization with constraint criteria

IV. CONCLUSION AND FUTURE WORK

A concept for a complete modeling of an autonomousunderwater vehicle was presented. Based on this optimizedmodel, an autopilot was designed, whose functionality hasbeen verified in sea trials. The procedure can be adapted toother underwater vehicles and provides an applicable template.

The description of the hydrodynamic variables of the modeland the designing of the controller parameters could be opti-mized by conducting extensive sea trials. The current status isbased on only two weeks with realistic experiments.

In addition to the application as a simulator, it’s conceivableto use the vehicle model within a Kalman filter to improve thenavigation. The robustness and insensitivity to disturbancescould be increased by the implementation of more advancedcontrol concepts in the autopilot.

REFERENCES

[1] Exzellenzcluster Ozean der Zukunft. (2013) World Ocean Review2: Die Zukunft der Fische – die Fischerei der Zukunft. [Online].Available: http://www.worldoceanreview.com

[2] Freistaat Thuringen. (2013) EFRE: Europaischer Fonds fur regionaleEntwicklung. [Online]. Available: http://www.thueringen.de/efre/

[3] M. Eichhorn, R. Taubert, C. Ament, M. Jacobi, and T. Pfuetzenreuter,“Modular AUV System for Sea Water Quality Monitoring and Manage-ment,” in Oceans ’13 Proceedings of MTS/IEEE, 2013.

[4] C. Ament. (2013) FG Systemanalyse: Fakultat fur Informatik undAutomatisierung, Technische Universitat Ilmenau. [Online]. Available:http://www.tu-ilmenau.de/systemanalyse/

[5] T. Pfuetzenreuter. (2013) Fraunhofer Institute of Optronics, SystemTechnologies and Image Exploitation: Maritime Systems. [Online].Available: http://www.iosb.fraunhofer.de/servlet/is/15004/

[6] M. Grunwald. (2013) -4H- JENA engineering GmbH: MaritimeTechnologien. [Online]. Available: http://www.4h-jena.de/index.php?id=11&L=%271

[7] K. O. Toft. (2013) Institute of Marine Research: Havforskningsinstituttet,Bergen. [Online]. Available: http://www.imr.no/en

[8] S. Konnecke. (2013) ATLAS Elektronik Group: ATLAS Hydrogrphic,Bremen. [Online]. Available: http://www.atlashydro.atlas-elektronik.com/company-info/

[9] T. Pfuetzenreuter and H. Renkewitz, “ConSys - A new software frame-work for underwater vehicles,” in OCEANS 2010 IEEE - Sydney. IEEE,2010.

[10] T. I. Fossen, Guidance and control of ocean vehicles. Chichester: Wiley,1994.

[11] T. I. Fossen and A. Ross, “Nonlinear modelling, identification andcontrol of UUVs,” in Advances in Unmanned Marine Vehicles, ser. IEEcontrol engineering series, G. N. Roberts, Ed. Institution of ElectricalEngineers, 2006, vol. 69, pp. 13–42.

[12] T. I. Fossen, Marine control systems: Guidance, navigation and controlof ships, rigs and underwater vehicles, 3rd ed. Trondheim: MarineCybernetics, 2002.

[13] G. Antonelli, Underwater robots: Motion and force control of vehicle-manipulator systems: Univ., Ph.D. Thesis–Neapel, 1999, ser. Springertracts in advanced robotics. Berlin: Springer, 2003, vol. 2.

[14] B. Ferreira, M. Pinto, A. Matos, and N. Cruz, “Modeling andmotion analysis of the MARES autonomous underwater vehicle,” inOCEANS 2009, MTS/IEEE Biloxi - Marine Technology for Our Future:Global and Local Challenges, 2009, pp. 1–10. [Online]. Available:http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5422130

[15] S. F. Hoerner, Fluid Dynamic Drag: theoretical, experimental andstatistical information. Bakersfield: Hoerner Fluid Dynamics, 1965.

[16] E. V. Lewis, Principles of Naval Architecture: Volume 2 - ResistancePropulsion Vibration. Jersey City: The Society of Naval Architects andMarine Engineers, 1988.

[17] F. M. White, Fluid mechanics, 7th ed., ser. McGraw-Hill Series inMechanical Engineering. New York: McGraw Hill, op. 2011.

[18] T. Prestero, “Verification of a Six-Degree of Freedom Simulation Modelfor the REMUS Autonomous Underwater Vehicle,” master thesis, Mas-sachusetts Institute of Technology, Cambridge, 27.11.2001.

[19] D. Karimanzira, M. Jacobi, T. Pfuetzenreuter, M. Eichhorn, R. Taubert,and C. Ament, “AUV Mission (Re)Planning and Guidance Concept forWater Quality Monitoring,” in The Seventh International Conference onComputer & Computing Technologies in Agriculture, 2013.

[20] R. Isermann, Digitale Regelsysteme: Grundlagen, DeterministischeRegelungen, 2nd ed. Berlin [u.a.]: Springer, 1988, vol. 1.

[21] M. Eichhorn, T. Pfuetzenreuter, M. Schmidt, and B. Waltl, “Model-lierung, Regelung und Navigation von teilautonomen, ferngesteuertenUnterwasserfahrzeugen: Modelling, control and navigation of semiau-tonomous, remotely operated underwater vehicles,” Automatisierung-stechnik : at ; Methoden und Anwendungen der Steuerungs-, Regelungs-und Informationstechnik ; Organ der GMA (VDI/VDE-GesellschaftMess- und Automatisierungstechnik) und der NAMUR (Interessenge-meinschaft Prozessleittechnik der Chemischen und PharmazeutischenIndustrie), vol. 49, no. 8, pp. 373–380, 2001.